From 64aaa21fe4701f5ead73b0a316dcd685257dde4e Mon Sep 17 00:00:00 2001 From: Maxwell Morgan Date: Wed, 14 Jan 2026 19:53:32 -0500 Subject: [PATCH 01/61] Create intake subsystem and io layers --- src/main/java/frc/robot/Constants.java | 282 +++++++++--------- src/main/java/frc/robot/RobotContainer.java | 6 +- .../frc/robot/subsystems/intake/Intake.java | 17 ++ .../frc/robot/subsystems/intake/IntakeIO.java | 28 ++ .../robot/subsystems/intake/IntakeIOSim.java | 5 + .../subsystems/intake/IntakeIOSparkMax.java | 33 ++ 6 files changed, 231 insertions(+), 140 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/intake/Intake.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIO.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 60a7e4d..8c415d6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -17,151 +17,159 @@ import java.util.Map; /** - * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running - * on a roboRIO. Change the value of "simMode" to switch between "sim" (physics sim) and "replay" + * This class defines the runtime mode used by AdvantageKit. The mode is always + * "real" when running + * on a roboRIO. Change the value of "simMode" to switch between "sim" (physics + * sim) and "replay" * (log replay from a file). */ public final class Constants { - public static final double kLoopPeriodSeconds = 0.02; + public static final double kLoopPeriodSeconds = 0.02; - public static final Mode kSimMode = Mode.SIM; - public static final Mode kCurrentMode = RobotBase.isReal() ? Mode.REAL : kSimMode; + public static final Mode kSimMode = Mode.SIM; + public static final Mode kCurrentMode = RobotBase.isReal() ? Mode.REAL : kSimMode; - public static enum Mode { - /** Running on a real robot. */ - REAL, + public static enum Mode { + /** Running on a real robot. */ + REAL, - /** Running a physics simulator. */ - SIM, + /** Running a physics simulator. */ + SIM, - /** Replaying from a log file. */ - REPLAY - } - - public class DriveConstants { - - public static class ModuleConfigs { - - public static record ModuleConfig( - int driveMotorID, int angleMotorID, int canCoderID, Rotation2d angleOffset) {} - - /** Module 0 (front left) configs. */ - public static final ModuleConfig FrontLeft = - new ModuleConfig(1, 2, 19, Rotation2d.fromDegrees(304.36523 - 180)); - - /** Module 1 (front right) configs. */ - public static final ModuleConfig FrontRight = - new ModuleConfig(2, 4, 20, Rotation2d.fromDegrees(206.455)); - - /** Module 2 (back left) configs. */ - public static final ModuleConfig BackLeft = - new ModuleConfig(5, 6, 21, Rotation2d.fromDegrees(35.419922 + 180)); + /** Replaying from a log file. */ + REPLAY + } - /** Module 3 (back right) configs. */ - public static final ModuleConfig BackRight = - new ModuleConfig(7, 8, 22, Rotation2d.fromDegrees(116.89453)); + public class DriveConstants { + + public static class ModuleConfigs { + + public static record ModuleConfig( + int driveMotorID, int angleMotorID, int canCoderID, Rotation2d angleOffset) { + } + + /** Module 0 (front left) configs. */ + public static final ModuleConfig FrontLeft = new ModuleConfig(1, 2, 19, + Rotation2d.fromDegrees(304.36523 - 180)); + + /** Module 1 (front right) configs. */ + public static final ModuleConfig FrontRight = new ModuleConfig(2, 4, 20, Rotation2d.fromDegrees(206.455)); + + /** Module 2 (back left) configs. */ + public static final ModuleConfig BackLeft = new ModuleConfig(5, 6, 21, + Rotation2d.fromDegrees(35.419922 + 180)); + + /** Module 3 (back right) configs. */ + public static final ModuleConfig BackRight = new ModuleConfig(7, 8, 22, Rotation2d.fromDegrees(116.89453)); + } + + public static final IdleMode kDriveIdleMode = IdleMode.kBrake; + public static final IdleMode kAngleIdleMode = IdleMode.kBrake; + public static final double kDrivePower = 1; + public static final double kAnglePower = .9; + + public static final boolean kInvertGyro = false; // Always ensure Gyro is CCW+ CW- + + // drivetrain constants + public static final double kTrackWidth = Units.inchesToMeters(24.75); + public static final double kWheelBase = Units.inchesToMeters(24.75); + public static final double kWheelDiameter = Units.inchesToMeters(4.0); + public static final double kWheelRadius = kWheelDiameter / 2.0; + public static final double kWheelCircumference = kWheelDiameter * Math.PI; + + // Swerve kinematics, don't change + public static final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics( + new Translation2d(kWheelBase / 2.0, kTrackWidth / 2.0), // front left + new Translation2d(kWheelBase / 2.0, -kTrackWidth / 2.0), // front right + new Translation2d(-kWheelBase / 2.0, kTrackWidth / 2.0), // back left + new Translation2d(-kWheelBase / 2.0, -kTrackWidth / 2.0)); // back right + + // gear ratios + public static final double kDriveGearRatio = (6.12 / 1.0); + public static final double kAngleGearRatio = ((150.0 / 7.0) / 1.0); + + // encoder stuff + // meters per rotation + public static final double kDriveRevToMeters = kWheelCircumference / (kDriveGearRatio); + public static final double kDriveRpmToMetersPerSecond = kDriveRevToMeters / 60; + + /** + * The number of degrees that a single rotation of the turn motor turns the // + * wheel. + */ + public static final double kDegreesPerTurnRotation = 360 / kAngleGearRatio; + + // motor inverts, check these + public static final boolean kAngleMotorInvert = true; + public static final InvertedValue kDriveMotorInvert = InvertedValue.CounterClockwise_Positive; + + /* Angle Encoder Invert */ + public static final boolean kCanCoderInvert = false; + + /* Swerve Current Limiting */ + public static final int kAngleContinuousCurrentLimit = 20; + public static final int kAnglePeakCurrentLimit = 40; + public static final double kAnglePeakCurrentDuration = 0.1; + public static final boolean kAngleEnableCurrentLimit = true; + + public static final int kDriveSupplyCurrentLimit = 60; + public static final boolean kDriveSupplyCurrentLimitEnable = true; + public static final int kDriveSupplyCurrentThreshold = 60; + public static final double kDriveSupplyTimeThreshold = 0.1; + + public static final boolean kDriveEnableCurrentLimit = true; + + /* + * These values are used by the drive falcon to ramp in open loop and closed + * loop driving. + * We found a small open loop ramp (0.25) helps with tread wear, tipping, etc + */ + public static final double kOpenLoopRamp = 0.25; + public static final double kClosedLoopRamp = 0.0; + + /* Angle Motor PID Values */ + public static final double kAngleKP = 0.015; + public static final double kAngleKI = 0; + public static final double kAngleKD = 0; + public static final double kAngleKF = 0; + + /* Drive Motor PID Values */ + + public static final double kDriveKP = 0.01; + public static final double kDriveKI = 0.0; + public static final double kDriveKD = 0.0; + + public static final double kDriveKS = (0.32 / 12); + public static final double kDriveKV = (1.988 / 12); + public static final double kDriveKA = (1.0449 / 12); + + /* Swerve Profiling Values */ + /** Meters per second. */ + public static final double kPhysicalMaxSpeed = 5.0; + + public static final double kMaxTeleDriveSpeed = 4.5; + /** Radians per second. */ + public static final double kPhysicalMaxAngularSpeed = 2 * 2 * Math.PI; + /** Radians per second. */ + public static final double kMaxTeleAngularSpeed = kPhysicalMaxAngularSpeed / 2; + + public static final double kMaxAngularAccelerationSpeed = 4 / Math.PI; + /** Radians per second. */ + public static final double kMaxTeleAngularAccelerationSpeed = kMaxAngularAccelerationSpeed / 2; + + public static final double kDeadband = 0.08; + + public static final Map kDistances = Map.of( + 0, 0.0, + 1, 1.0, + 2, 2.0, + 3, 3.0, + 4, 4.0); } - public static final IdleMode kDriveIdleMode = IdleMode.kBrake; - public static final IdleMode kAngleIdleMode = IdleMode.kBrake; - public static final double kDrivePower = 1; - public static final double kAnglePower = .9; - - public static final boolean kInvertGyro = false; // Always ensure Gyro is CCW+ CW- - - // drivetrain constants - public static final double kTrackWidth = Units.inchesToMeters(24.75); - public static final double kWheelBase = Units.inchesToMeters(24.75); - public static final double kWheelDiameter = Units.inchesToMeters(4.0); - public static final double kWheelRadius = kWheelDiameter / 2.0; - public static final double kWheelCircumference = kWheelDiameter * Math.PI; - - // Swerve kinematics, don't change - public static final SwerveDriveKinematics swerveKinematics = - new SwerveDriveKinematics( - new Translation2d(kWheelBase / 2.0, kTrackWidth / 2.0), // front left - new Translation2d(kWheelBase / 2.0, -kTrackWidth / 2.0), // front right - new Translation2d(-kWheelBase / 2.0, kTrackWidth / 2.0), // back left - new Translation2d(-kWheelBase / 2.0, -kTrackWidth / 2.0)); // back right - - // gear ratios - public static final double kDriveGearRatio = (6.12 / 1.0); - public static final double kAngleGearRatio = ((150.0 / 7.0) / 1.0); - - // encoder stuff - // meters per rotation - public static final double kDriveRevToMeters = kWheelCircumference / (kDriveGearRatio); - public static final double kDriveRpmToMetersPerSecond = kDriveRevToMeters / 60; - - /** The number of degrees that a single rotation of the turn motor turns the // wheel. */ - public static final double kDegreesPerTurnRotation = 360 / kAngleGearRatio; - - // motor inverts, check these - public static final boolean kAngleMotorInvert = true; - public static final InvertedValue kDriveMotorInvert = InvertedValue.CounterClockwise_Positive; - - /* Angle Encoder Invert */ - public static final boolean kCanCoderInvert = false; - - /* Swerve Current Limiting */ - public static final int kAngleContinuousCurrentLimit = 20; - public static final int kAnglePeakCurrentLimit = 40; - public static final double kAnglePeakCurrentDuration = 0.1; - public static final boolean kAngleEnableCurrentLimit = true; - - public static final int kDriveSupplyCurrentLimit = 60; - public static final boolean kDriveSupplyCurrentLimitEnable = true; - public static final int kDriveSupplyCurrentThreshold = 60; - public static final double kDriveSupplyTimeThreshold = 0.1; - - public static final boolean kDriveEnableCurrentLimit = true; - - /* - * These values are used by the drive falcon to ramp in open loop and closed - * loop driving. - * We found a small open loop ramp (0.25) helps with tread wear, tipping, etc - */ - public static final double kOpenLoopRamp = 0.25; - public static final double kClosedLoopRamp = 0.0; - - /* Angle Motor PID Values */ - public static final double kAngleKP = 0.015; - public static final double kAngleKI = 0; - public static final double kAngleKD = 0; - public static final double kAngleKF = 0; - - /* Drive Motor PID Values */ - - public static final double kDriveKP = 0.01; - public static final double kDriveKI = 0.0; - public static final double kDriveKD = 0.0; - - public static final double kDriveKS = (0.32 / 12); - public static final double kDriveKV = (1.988 / 12); - public static final double kDriveKA = (1.0449 / 12); - - /* Swerve Profiling Values */ - /** Meters per second. */ - public static final double kPhysicalMaxSpeed = 5.0; - - public static final double kMaxTeleDriveSpeed = 4.5; - /** Radians per second. */ - public static final double kPhysicalMaxAngularSpeed = 2 * 2 * Math.PI; - /** Radians per second. */ - public static final double kMaxTeleAngularSpeed = kPhysicalMaxAngularSpeed / 2; - - public static final double kMaxAngularAccelerationSpeed = 4 / Math.PI; - /** Radians per second. */ - public static final double kMaxTeleAngularAccelerationSpeed = kMaxAngularAccelerationSpeed / 2; - - public static final double kDeadband = 0.08; - - public static final Map kDistances = - Map.of( - 0, 0.0, - 1, 1.0, - 2, 2.0, - 3, 3.0, - 4, 4.0); - } + public static class IntakeConstants { + public static final int kPivotMotorID = 8; + public static final int kRollerMotorID = 9; + + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5d5bafb..9103698 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,8 @@ package frc.robot; +import java.security.Timestamp; + import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; @@ -50,9 +52,7 @@ private void configureBindings() { } public void robotPeriodic() { - OdometryObservation obs = - new OdometryObservation( - Timer.getTimestamp(), drive.getModulePositions(), drive.getRawGyroRotation()); + OdometryObservation obs = new OdometryObservation(Timer.getTimestamp(), drive.getModulePositions(), drive.getRawGyroRotation()); RobotState.getInstance().addOdometryObservation(obs); } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java new file mode 100644 index 0000000..145f3f1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -0,0 +1,17 @@ +// 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.intake; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Intake extends SubsystemBase { + /** Creates a new Intake. */ + public Intake() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java new file mode 100644 index 0000000..f163c25 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -0,0 +1,28 @@ +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.AutoLog; + +/** + * The {@code IntakeIO} class provides methods for interacting with the intake + * motors and updating the intake inputs. + * + * @author Ryan Hefferon + * @author Matthew McGrath + * @author Maxwell Morgan + * @author Julien Precourt + */ +public interface IntakeIO { + default void updateInputs(IntakeIOInputs inputs) { + } + + @AutoLog + public class IntakeIOInputs { + + } + + default void runPivotMotor(double speed) { + } + + default void runRollerMotor(double speed) { + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java new file mode 100644 index 0000000..3f307e3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.intake; + +public class IntakeIOSim implements IntakeIO { + +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java new file mode 100644 index 0000000..804997b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java @@ -0,0 +1,33 @@ +package frc.robot.subsystems.intake; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import frc.robot.Constants.IntakeConstants; + +public class IntakeIOSparkMax implements IntakeIO { + + private SparkMax pivotMotor = new SparkMax(IntakeConstants.kPivotMotorID, MotorType.kBrushless); + private SparkMax rollerMotor = new SparkMax(IntakeConstants.kRollerMotorID, MotorType.kBrushless); + + private RelativeEncoder pivotEncoder = pivotMotor.getEncoder(); + private RelativeEncoder rollerEncoder = rollerMotor.getEncoder(); + + public IntakeIOSparkMax() {} + + @Override + public void updateInputs(IntakeIOInputs inputs) { + + } + + @Override + public void runPivotMotor(double speed) { + pivotMotor.set(speed); + } + + @Override + public void runRollerMotor(double speed) { + rollerMotor.set(speed); + } +} From 25b379c2234d4cc3a849c861fb519c020fbb5a45 Mon Sep 17 00:00:00 2001 From: Maxwell Morgan Date: Wed, 14 Jan 2026 21:21:21 -0500 Subject: [PATCH 02/61] Update team number --- .wpilib/wpilib_preferences.json | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 97e9317..81fa8f1 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { - "enableCppIntellisense": false, - "currentLanguage": "java", - "projectYear": "2026", - "teamNumber": 6328 -} + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2026", + "teamNumber": 3464 +} \ No newline at end of file From 49b6d320419b90297a30877a13eb7aa8f2db14bb Mon Sep 17 00:00:00 2001 From: Maxwell Morgan Date: Fri, 16 Jan 2026 16:28:56 -0500 Subject: [PATCH 03/61] Add Intake subsystem and IO interface(s) --- .wpilib/wpilib_preferences.json | 10 +- README.md | 2 +- src/main/java/frc/robot/Constants.java | 287 +++++++++--------- src/main/java/frc/robot/RobotContainer.java | 6 +- .../frc/robot/subsystems/intake/IntakeIO.java | 21 +- .../robot/subsystems/intake/IntakeIOSim.java | 4 +- .../subsystems/intake/IntakeIOSparkMax.java | 35 +-- 7 files changed, 176 insertions(+), 189 deletions(-) diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 81fa8f1..8338809 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { - "enableCppIntellisense": false, - "currentLanguage": "java", - "projectYear": "2026", - "teamNumber": 3464 -} \ No newline at end of file + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2026", + "teamNumber": 3464 +} diff --git a/README.md b/README.md index bae1d67..4d47aed 100644 --- a/README.md +++ b/README.md @@ -1 +1 @@ -# FRC Team 3464 "Sim-City" 2026 Robot Code \ No newline at end of file +# FRC Team 3464 "Sim-City" 2026 Robot Code diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8c415d6..d3ef038 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -17,159 +17,156 @@ import java.util.Map; /** - * This class defines the runtime mode used by AdvantageKit. The mode is always - * "real" when running - * on a roboRIO. Change the value of "simMode" to switch between "sim" (physics - * sim) and "replay" + * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running + * on a roboRIO. Change the value of "simMode" to switch between "sim" (physics sim) and "replay" * (log replay from a file). */ public final class Constants { - public static final double kLoopPeriodSeconds = 0.02; + public static final double kLoopPeriodSeconds = 0.02; - public static final Mode kSimMode = Mode.SIM; - public static final Mode kCurrentMode = RobotBase.isReal() ? Mode.REAL : kSimMode; + public static final Mode kSimMode = Mode.SIM; + public static final Mode kCurrentMode = RobotBase.isReal() ? Mode.REAL : kSimMode; - public static enum Mode { - /** Running on a real robot. */ - REAL, + public static enum Mode { + /** Running on a real robot. */ + REAL, - /** Running a physics simulator. */ - SIM, + /** Running a physics simulator. */ + SIM, - /** Replaying from a log file. */ - REPLAY - } + /** Replaying from a log file. */ + REPLAY + } - public class DriveConstants { - - public static class ModuleConfigs { - - public static record ModuleConfig( - int driveMotorID, int angleMotorID, int canCoderID, Rotation2d angleOffset) { - } - - /** Module 0 (front left) configs. */ - public static final ModuleConfig FrontLeft = new ModuleConfig(1, 2, 19, - Rotation2d.fromDegrees(304.36523 - 180)); - - /** Module 1 (front right) configs. */ - public static final ModuleConfig FrontRight = new ModuleConfig(2, 4, 20, Rotation2d.fromDegrees(206.455)); - - /** Module 2 (back left) configs. */ - public static final ModuleConfig BackLeft = new ModuleConfig(5, 6, 21, - Rotation2d.fromDegrees(35.419922 + 180)); - - /** Module 3 (back right) configs. */ - public static final ModuleConfig BackRight = new ModuleConfig(7, 8, 22, Rotation2d.fromDegrees(116.89453)); - } - - public static final IdleMode kDriveIdleMode = IdleMode.kBrake; - public static final IdleMode kAngleIdleMode = IdleMode.kBrake; - public static final double kDrivePower = 1; - public static final double kAnglePower = .9; - - public static final boolean kInvertGyro = false; // Always ensure Gyro is CCW+ CW- - - // drivetrain constants - public static final double kTrackWidth = Units.inchesToMeters(24.75); - public static final double kWheelBase = Units.inchesToMeters(24.75); - public static final double kWheelDiameter = Units.inchesToMeters(4.0); - public static final double kWheelRadius = kWheelDiameter / 2.0; - public static final double kWheelCircumference = kWheelDiameter * Math.PI; - - // Swerve kinematics, don't change - public static final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics( - new Translation2d(kWheelBase / 2.0, kTrackWidth / 2.0), // front left - new Translation2d(kWheelBase / 2.0, -kTrackWidth / 2.0), // front right - new Translation2d(-kWheelBase / 2.0, kTrackWidth / 2.0), // back left - new Translation2d(-kWheelBase / 2.0, -kTrackWidth / 2.0)); // back right - - // gear ratios - public static final double kDriveGearRatio = (6.12 / 1.0); - public static final double kAngleGearRatio = ((150.0 / 7.0) / 1.0); - - // encoder stuff - // meters per rotation - public static final double kDriveRevToMeters = kWheelCircumference / (kDriveGearRatio); - public static final double kDriveRpmToMetersPerSecond = kDriveRevToMeters / 60; - - /** - * The number of degrees that a single rotation of the turn motor turns the // - * wheel. - */ - public static final double kDegreesPerTurnRotation = 360 / kAngleGearRatio; - - // motor inverts, check these - public static final boolean kAngleMotorInvert = true; - public static final InvertedValue kDriveMotorInvert = InvertedValue.CounterClockwise_Positive; - - /* Angle Encoder Invert */ - public static final boolean kCanCoderInvert = false; - - /* Swerve Current Limiting */ - public static final int kAngleContinuousCurrentLimit = 20; - public static final int kAnglePeakCurrentLimit = 40; - public static final double kAnglePeakCurrentDuration = 0.1; - public static final boolean kAngleEnableCurrentLimit = true; - - public static final int kDriveSupplyCurrentLimit = 60; - public static final boolean kDriveSupplyCurrentLimitEnable = true; - public static final int kDriveSupplyCurrentThreshold = 60; - public static final double kDriveSupplyTimeThreshold = 0.1; - - public static final boolean kDriveEnableCurrentLimit = true; - - /* - * These values are used by the drive falcon to ramp in open loop and closed - * loop driving. - * We found a small open loop ramp (0.25) helps with tread wear, tipping, etc - */ - public static final double kOpenLoopRamp = 0.25; - public static final double kClosedLoopRamp = 0.0; - - /* Angle Motor PID Values */ - public static final double kAngleKP = 0.015; - public static final double kAngleKI = 0; - public static final double kAngleKD = 0; - public static final double kAngleKF = 0; - - /* Drive Motor PID Values */ - - public static final double kDriveKP = 0.01; - public static final double kDriveKI = 0.0; - public static final double kDriveKD = 0.0; - - public static final double kDriveKS = (0.32 / 12); - public static final double kDriveKV = (1.988 / 12); - public static final double kDriveKA = (1.0449 / 12); - - /* Swerve Profiling Values */ - /** Meters per second. */ - public static final double kPhysicalMaxSpeed = 5.0; - - public static final double kMaxTeleDriveSpeed = 4.5; - /** Radians per second. */ - public static final double kPhysicalMaxAngularSpeed = 2 * 2 * Math.PI; - /** Radians per second. */ - public static final double kMaxTeleAngularSpeed = kPhysicalMaxAngularSpeed / 2; - - public static final double kMaxAngularAccelerationSpeed = 4 / Math.PI; - /** Radians per second. */ - public static final double kMaxTeleAngularAccelerationSpeed = kMaxAngularAccelerationSpeed / 2; - - public static final double kDeadband = 0.08; - - public static final Map kDistances = Map.of( - 0, 0.0, - 1, 1.0, - 2, 2.0, - 3, 3.0, - 4, 4.0); - } + public class DriveConstants { + + public static class ModuleConfigs { + + public static record ModuleConfig( + int driveMotorID, int angleMotorID, int canCoderID, Rotation2d angleOffset) {} - public static class IntakeConstants { - public static final int kPivotMotorID = 8; - public static final int kRollerMotorID = 9; - + /** Module 0 (front left) configs. */ + public static final ModuleConfig FrontLeft = + new ModuleConfig(1, 2, 19, Rotation2d.fromDegrees(304.36523 - 180)); + + /** Module 1 (front right) configs. */ + public static final ModuleConfig FrontRight = + new ModuleConfig(2, 4, 20, Rotation2d.fromDegrees(206.455)); + + /** Module 2 (back left) configs. */ + public static final ModuleConfig BackLeft = + new ModuleConfig(5, 6, 21, Rotation2d.fromDegrees(35.419922 + 180)); + + /** Module 3 (back right) configs. */ + public static final ModuleConfig BackRight = + new ModuleConfig(7, 8, 22, Rotation2d.fromDegrees(116.89453)); } + + public static final IdleMode kDriveIdleMode = IdleMode.kBrake; + public static final IdleMode kAngleIdleMode = IdleMode.kBrake; + public static final double kDrivePower = 1; + public static final double kAnglePower = .9; + + public static final boolean kInvertGyro = false; // Always ensure Gyro is CCW+ CW- + + // drivetrain constants + public static final double kTrackWidth = Units.inchesToMeters(24.75); + public static final double kWheelBase = Units.inchesToMeters(24.75); + public static final double kWheelDiameter = Units.inchesToMeters(4.0); + public static final double kWheelRadius = kWheelDiameter / 2.0; + public static final double kWheelCircumference = kWheelDiameter * Math.PI; + + // Swerve kinematics, don't change + public static final SwerveDriveKinematics swerveKinematics = + new SwerveDriveKinematics( + new Translation2d(kWheelBase / 2.0, kTrackWidth / 2.0), // front left + new Translation2d(kWheelBase / 2.0, -kTrackWidth / 2.0), // front right + new Translation2d(-kWheelBase / 2.0, kTrackWidth / 2.0), // back left + new Translation2d(-kWheelBase / 2.0, -kTrackWidth / 2.0)); // back right + + // gear ratios + public static final double kDriveGearRatio = (6.12 / 1.0); + public static final double kAngleGearRatio = ((150.0 / 7.0) / 1.0); + + // encoder stuff + // meters per rotation + public static final double kDriveRevToMeters = kWheelCircumference / (kDriveGearRatio); + public static final double kDriveRpmToMetersPerSecond = kDriveRevToMeters / 60; + + /** The number of degrees that a single rotation of the turn motor turns the // wheel. */ + public static final double kDegreesPerTurnRotation = 360 / kAngleGearRatio; + + // motor inverts, check these + public static final boolean kAngleMotorInvert = true; + public static final InvertedValue kDriveMotorInvert = InvertedValue.CounterClockwise_Positive; + + /* Angle Encoder Invert */ + public static final boolean kCanCoderInvert = false; + + /* Swerve Current Limiting */ + public static final int kAngleContinuousCurrentLimit = 20; + public static final int kAnglePeakCurrentLimit = 40; + public static final double kAnglePeakCurrentDuration = 0.1; + public static final boolean kAngleEnableCurrentLimit = true; + + public static final int kDriveSupplyCurrentLimit = 60; + public static final boolean kDriveSupplyCurrentLimitEnable = true; + public static final int kDriveSupplyCurrentThreshold = 60; + public static final double kDriveSupplyTimeThreshold = 0.1; + + public static final boolean kDriveEnableCurrentLimit = true; + + /* + * These values are used by the drive falcon to ramp in open loop and closed + * loop driving. + * We found a small open loop ramp (0.25) helps with tread wear, tipping, etc + */ + public static final double kOpenLoopRamp = 0.25; + public static final double kClosedLoopRamp = 0.0; + + /* Angle Motor PID Values */ + public static final double kAngleKP = 0.015; + public static final double kAngleKI = 0; + public static final double kAngleKD = 0; + public static final double kAngleKF = 0; + + /* Drive Motor PID Values */ + + public static final double kDriveKP = 0.01; + public static final double kDriveKI = 0.0; + public static final double kDriveKD = 0.0; + + public static final double kDriveKS = (0.32 / 12); + public static final double kDriveKV = (1.988 / 12); + public static final double kDriveKA = (1.0449 / 12); + + /* Swerve Profiling Values */ + /** Meters per second. */ + public static final double kPhysicalMaxSpeed = 5.0; + + public static final double kMaxTeleDriveSpeed = 4.5; + /** Radians per second. */ + public static final double kPhysicalMaxAngularSpeed = 2 * 2 * Math.PI; + /** Radians per second. */ + public static final double kMaxTeleAngularSpeed = kPhysicalMaxAngularSpeed / 2; + + public static final double kMaxAngularAccelerationSpeed = 4 / Math.PI; + /** Radians per second. */ + public static final double kMaxTeleAngularAccelerationSpeed = kMaxAngularAccelerationSpeed / 2; + + public static final double kDeadband = 0.08; + + public static final Map kDistances = + Map.of( + 0, 0.0, + 1, 1.0, + 2, 2.0, + 3, 3.0, + 4, 4.0); + } + + public static class IntakeConstants { + public static final int kPivotMotorID = 8; + public static final int kRollerMotorID = 9; + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9103698..5d5bafb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,8 +4,6 @@ package frc.robot; -import java.security.Timestamp; - import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; @@ -52,7 +50,9 @@ private void configureBindings() { } public void robotPeriodic() { - OdometryObservation obs = new OdometryObservation(Timer.getTimestamp(), drive.getModulePositions(), drive.getRawGyroRotation()); + OdometryObservation obs = + new OdometryObservation( + Timer.getTimestamp(), drive.getModulePositions(), drive.getRawGyroRotation()); RobotState.getInstance().addOdometryObservation(obs); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index f163c25..d9250b4 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -3,26 +3,21 @@ import org.littletonrobotics.junction.AutoLog; /** - * The {@code IntakeIO} class provides methods for interacting with the intake - * motors and updating the intake inputs. - * + * The {@code IntakeIO} class provides methods for interacting with the intake motors and updating + * the intake inputs. + * * @author Ryan Hefferon * @author Matthew McGrath * @author Maxwell Morgan * @author Julien Precourt */ public interface IntakeIO { - default void updateInputs(IntakeIOInputs inputs) { - } + default void updateInputs(IntakeIOInputs inputs) {} - @AutoLog - public class IntakeIOInputs { + @AutoLog + public class IntakeIOInputs {} - } + default void runPivotMotor(double speed) {} - default void runPivotMotor(double speed) { - } - - default void runRollerMotor(double speed) { - } + default void runRollerMotor(double speed) {} } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 3f307e3..6c412da 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -1,5 +1,3 @@ package frc.robot.subsystems.intake; -public class IntakeIOSim implements IntakeIO { - -} +public class IntakeIOSim implements IntakeIO {} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java index 804997b..69dd1a9 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java @@ -1,33 +1,30 @@ package frc.robot.subsystems.intake; import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; - +import com.revrobotics.spark.SparkMax; import frc.robot.Constants.IntakeConstants; public class IntakeIOSparkMax implements IntakeIO { - private SparkMax pivotMotor = new SparkMax(IntakeConstants.kPivotMotorID, MotorType.kBrushless); - private SparkMax rollerMotor = new SparkMax(IntakeConstants.kRollerMotorID, MotorType.kBrushless); + private SparkMax pivotMotor = new SparkMax(IntakeConstants.kPivotMotorID, MotorType.kBrushless); + private SparkMax rollerMotor = new SparkMax(IntakeConstants.kRollerMotorID, MotorType.kBrushless); - private RelativeEncoder pivotEncoder = pivotMotor.getEncoder(); - private RelativeEncoder rollerEncoder = rollerMotor.getEncoder(); - - public IntakeIOSparkMax() {} + private RelativeEncoder pivotEncoder = pivotMotor.getEncoder(); + private RelativeEncoder rollerEncoder = rollerMotor.getEncoder(); - @Override - public void updateInputs(IntakeIOInputs inputs) { + public IntakeIOSparkMax() {} - } + @Override + public void updateInputs(IntakeIOInputs inputs) {} - @Override - public void runPivotMotor(double speed) { - pivotMotor.set(speed); - } + @Override + public void runPivotMotor(double speed) { + pivotMotor.set(speed); + } - @Override - public void runRollerMotor(double speed) { - rollerMotor.set(speed); - } + @Override + public void runRollerMotor(double speed) { + rollerMotor.set(speed); + } } From 869e5240898b99221587fc82eb9a43d356b8e8ec Mon Sep 17 00:00:00 2001 From: Maxwell Morgan Date: Mon, 19 Jan 2026 13:44:54 -0500 Subject: [PATCH 04/61] Add PhotonVision vendordep; add Vision/Camera classes from AdvantageKit template --- .wpilib/wpilib_preferences.json | 10 +- README.md | 2 +- src/main/java/frc/robot/Constants.java | 6 +- .../frc/robot/subsystems/vision/CameraIO.java | 43 + .../subsystems/vision/CameraIOLimelight.java | 153 ++ .../vision/CameraIOPhotonVision.java | 125 ++ .../vision/CameraIOPhotonVisionSim.java | 54 + .../frc/robot/subsystems/vision/Vision.java | 178 ++ .../subsystems/vision/VisionConstants.java | 52 + .../java/frc/robot/util/LimelightHelpers.java | 1692 +++++++++++++++++ vendordeps/photonlib.json | 71 + 11 files changed, 2378 insertions(+), 8 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/vision/CameraIO.java create mode 100644 src/main/java/frc/robot/subsystems/vision/CameraIOLimelight.java create mode 100644 src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVision.java create mode 100644 src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVisionSim.java create mode 100644 src/main/java/frc/robot/subsystems/vision/Vision.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionConstants.java create mode 100644 src/main/java/frc/robot/util/LimelightHelpers.java create mode 100644 vendordeps/photonlib.json diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 81fa8f1..8338809 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { - "enableCppIntellisense": false, - "currentLanguage": "java", - "projectYear": "2026", - "teamNumber": 3464 -} \ No newline at end of file + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2026", + "teamNumber": 3464 +} diff --git a/README.md b/README.md index bae1d67..4d47aed 100644 --- a/README.md +++ b/README.md @@ -1 +1 @@ -# FRC Team 3464 "Sim-City" 2026 Robot Code \ No newline at end of file +# FRC Team 3464 "Sim-City" 2026 Robot Code diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 60a7e4d..c14e9a1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -38,9 +38,9 @@ public static enum Mode { REPLAY } - public class DriveConstants { + public static final class DriveConstants { - public static class ModuleConfigs { + public static final class ModuleConfigs { public static record ModuleConfig( int driveMotorID, int angleMotorID, int canCoderID, Rotation2d angleOffset) {} @@ -164,4 +164,6 @@ public static record ModuleConfig( 3, 3.0, 4, 4.0); } + + public static final class VisionConstants {} } diff --git a/src/main/java/frc/robot/subsystems/vision/CameraIO.java b/src/main/java/frc/robot/subsystems/vision/CameraIO.java new file mode 100644 index 0000000..bd8f97a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/CameraIO.java @@ -0,0 +1,43 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface CameraIO { + public default void updateInputs(CameraIOInputs inputs) {} + + @AutoLog + public static class CameraIOInputs { + public boolean connected = false; + public TargetObservation latestTargetObservation = + new TargetObservation(Rotation2d.kZero, Rotation2d.kZero); + public PoseObservation[] poseObservations = new PoseObservation[0]; + public int[] tagIds = new int[0]; + } + + /** Represents the angle to a target. Not used for pose estimation */ + public static record TargetObservation(Rotation2d tx, Rotation2d ty) {} + + /** Represents a robot pose sample used for pose estimation. */ + public static record PoseObservation( + double timestamp, + Pose3d pose, + double ambiguity, + int tagCount, + double averageTagDistance, + PoseObservationType type) {} + + public static enum PoseObservationType { + MEGATAG_1, + MEGATAG_2, + PHOTONVISION + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/CameraIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/CameraIOLimelight.java new file mode 100644 index 0000000..f43f708 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/CameraIOLimelight.java @@ -0,0 +1,153 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.vision; + +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.util.Units; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.DoubleArraySubscriber; +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.RobotController; +import java.util.HashSet; +import java.util.LinkedList; +import java.util.List; +import java.util.Set; +import java.util.function.Supplier; + +/** {@code CameraIO} implementation for running on a real Limelight camera. */ +public class CameraIOLimelight implements CameraIO { + private final Supplier rotationSupplier; + private final DoubleArrayPublisher orientationPublisher; + + private final DoubleSubscriber latencySubscriber; + private final DoubleSubscriber txSubscriber; + private final DoubleSubscriber tySubscriber; + private final DoubleArraySubscriber megatag1Subscriber; + private final DoubleArraySubscriber megatag2Subscriber; + + /** + * Creates a new CameraIOLimelight. + * + * @param name The configured name of the Limelight camera. + * @param rotationSupplier Supplier for the current estimated rotation. + */ + public CameraIOLimelight(String name, Supplier rotationSupplier) { + var table = NetworkTableInstance.getDefault().getTable(name); + this.rotationSupplier = rotationSupplier; + this.orientationPublisher = table.getDoubleArrayTopic("robot_orientation_set").publish(); + this.latencySubscriber = table.getDoubleTopic("tl").subscribe(0.0); + this.txSubscriber = table.getDoubleTopic("tx").subscribe(0.0); + this.tySubscriber = table.getDoubleTopic("ty").subscribe(0.0); + this.megatag1Subscriber = + table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[] {}); + this.megatag2Subscriber = + table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[] {}); + } + + @Override + public void updateInputs(CameraIOInputs inputs) { + // Update connection status based on whether an update has been seen in the last + // 250ms + inputs.connected = + ((RobotController.getFPGATime() - latencySubscriber.getLastChange()) / 1000) < 250; + + // Update target observation + inputs.latestTargetObservation = + new TargetObservation( + Rotation2d.fromDegrees(txSubscriber.get()), Rotation2d.fromDegrees(tySubscriber.get())); + + // Update orientation for MegaTag 2 + orientationPublisher.accept( + new double[] {rotationSupplier.get().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0}); + NetworkTableInstance.getDefault() + .flush(); // Increases network traffic but recommended by Limelight + + // Read new pose observations from NetworkTables + Set tagIds = new HashSet<>(); + List poseObservations = new LinkedList<>(); + for (var rawSample : megatag1Subscriber.readQueue()) { + if (rawSample.value.length == 0) continue; + for (int i = 11; i < rawSample.value.length; i += 7) { + tagIds.add((int) rawSample.value[i]); + } + poseObservations.add( + new PoseObservation( + // Timestamp, based on server timestamp of publish and latency + rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, + + // 3D pose estimate + parsePose(rawSample.value), + + // Ambiguity, using only the first tag because ambiguity isn't applicable for + // multitag + rawSample.value.length >= 18 ? rawSample.value[17] : 0.0, + + // Tag count + (int) rawSample.value[7], + + // Average tag distance + rawSample.value[9], + + // Observation type + PoseObservationType.MEGATAG_1)); + } + for (var rawSample : megatag2Subscriber.readQueue()) { + if (rawSample.value.length == 0) continue; + for (int i = 11; i < rawSample.value.length; i += 7) { + tagIds.add((int) rawSample.value[i]); + } + poseObservations.add( + new PoseObservation( + // Timestamp, based on server timestamp of publish and latency + rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, + + // 3D pose estimate + parsePose(rawSample.value), + + // Ambiguity, zeroed because the pose is already disambiguated + 0.0, + + // Tag count + (int) rawSample.value[7], + + // Average tag distance + rawSample.value[9], + + // Observation type + PoseObservationType.MEGATAG_2)); + } + + // Save pose observations to inputs object + inputs.poseObservations = new PoseObservation[poseObservations.size()]; + for (int i = 0; i < poseObservations.size(); i++) { + inputs.poseObservations[i] = poseObservations.get(i); + } + + // Save tag IDs to inputs objects + inputs.tagIds = new int[tagIds.size()]; + int i = 0; + for (int id : tagIds) { + inputs.tagIds[i++] = id; + } + } + + /** Parses the 3D pose from a Limelight botpose array. */ + private static Pose3d parsePose(double[] rawLLArray) { + return new Pose3d( + rawLLArray[0], + rawLLArray[1], + rawLLArray[2], + new Rotation3d( + Units.degreesToRadians(rawLLArray[3]), + Units.degreesToRadians(rawLLArray[4]), + Units.degreesToRadians(rawLLArray[5]))); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVision.java new file mode 100644 index 0000000..0ad84c2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVision.java @@ -0,0 +1,125 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.vision; + +import static frc.robot.subsystems.vision.VisionConstants.*; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import java.util.HashSet; +import java.util.LinkedList; +import java.util.List; +import java.util.Set; +import org.photonvision.PhotonCamera; + +/** IO implementation for real PhotonVision hardware. */ +public class CameraIOPhotonVision implements CameraIO { + protected final PhotonCamera camera; + protected final Transform3d robotToCamera; + + /** + * Creates a new VisionIOPhotonVision. + * + * @param name The configured name of the camera. + * @param robotToCamera The 3D position of the camera relative to the robot. + */ + public CameraIOPhotonVision(String name, Transform3d robotToCamera) { + camera = new PhotonCamera(name); + this.robotToCamera = robotToCamera; + } + + @Override + public void updateInputs(CameraIOInputs inputs) { + inputs.connected = camera.isConnected(); + + // Read new camera observations + Set tagIds = new HashSet<>(); + List poseObservations = new LinkedList<>(); + for (var result : camera.getAllUnreadResults()) { + // Update latest target observation + if (result.hasTargets()) { + inputs.latestTargetObservation = + new TargetObservation( + Rotation2d.fromDegrees(result.getBestTarget().getYaw()), + Rotation2d.fromDegrees(result.getBestTarget().getPitch())); + } else { + inputs.latestTargetObservation = new TargetObservation(Rotation2d.kZero, Rotation2d.kZero); + } + + // Add pose observation + if (result.multitagResult.isPresent()) { // Multitag result + var multitagResult = result.multitagResult.get(); + + // Calculate robot pose + Transform3d fieldToCamera = multitagResult.estimatedPose.best; + Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); + Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + + // Calculate average tag distance + double totalTagDistance = 0.0; + for (var target : result.targets) { + totalTagDistance += target.bestCameraToTarget.getTranslation().getNorm(); + } + + // Add tag IDs + tagIds.addAll(multitagResult.fiducialIDsUsed); + + // Add observation + poseObservations.add( + new PoseObservation( + result.getTimestampSeconds(), // Timestamp + robotPose, // 3D pose estimate + multitagResult.estimatedPose.ambiguity, // Ambiguity + multitagResult.fiducialIDsUsed.size(), // Tag count + totalTagDistance / result.targets.size(), // Average tag distance + PoseObservationType.PHOTONVISION)); // Observation type + + } else if (!result.targets.isEmpty()) { // Single tag result + var target = result.targets.get(0); + + // Calculate robot pose + var tagPose = aprilTagLayout.getTagPose(target.fiducialId); + if (tagPose.isPresent()) { + Transform3d fieldToTarget = + new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation()); + Transform3d cameraToTarget = target.bestCameraToTarget; + Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse()); + Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); + Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + + // Add tag ID + tagIds.add((short) target.fiducialId); + + // Add observation + poseObservations.add( + new PoseObservation( + result.getTimestampSeconds(), // Timestamp + robotPose, // 3D pose estimate + target.poseAmbiguity, // Ambiguity + 1, // Tag count + cameraToTarget.getTranslation().getNorm(), // Average tag distance + PoseObservationType.PHOTONVISION)); // Observation type + } + } + } + + // Save pose observations to inputs object + inputs.poseObservations = new PoseObservation[poseObservations.size()]; + for (int i = 0; i < poseObservations.size(); i++) { + inputs.poseObservations[i] = poseObservations.get(i); + } + + // Save tag IDs to inputs objects + inputs.tagIds = new int[tagIds.size()]; + int i = 0; + for (int id : tagIds) { + inputs.tagIds[i++] = id; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVisionSim.java new file mode 100644 index 0000000..56e41da --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVisionSim.java @@ -0,0 +1,54 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.vision; + +import static frc.robot.subsystems.vision.VisionConstants.aprilTagLayout; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform3d; +import java.util.function.Supplier; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; + +/** IO implementation for physics sim using PhotonVision simulator. */ +public class CameraIOPhotonVisionSim extends CameraIOPhotonVision { + private static VisionSystemSim visionSim; + + private final Supplier poseSupplier; + private final PhotonCameraSim cameraSim; + + /** + * Creates a new VisionIOPhotonVisionSim. + * + * @param name The name of the camera. + * @param poseSupplier Supplier for the robot pose to use in simulation. + */ + public CameraIOPhotonVisionSim( + String name, Transform3d robotToCamera, Supplier poseSupplier) { + super(name, robotToCamera); + this.poseSupplier = poseSupplier; + + // Initialize vision sim + if (visionSim == null) { + visionSim = new VisionSystemSim("main"); + visionSim.addAprilTags(aprilTagLayout); + } + + // Add sim camera + var cameraProperties = new SimCameraProperties(); + cameraSim = new PhotonCameraSim(camera, cameraProperties, aprilTagLayout); + visionSim.addCamera(cameraSim, robotToCamera); + } + + @Override + public void updateInputs(CameraIOInputs inputs) { + visionSim.update(poseSupplier.get()); + super.updateInputs(inputs); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java new file mode 100644 index 0000000..7b9e651 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -0,0 +1,178 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.vision; + +import static frc.robot.subsystems.vision.VisionConstants.*; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.vision.CameraIO.PoseObservationType; +import java.util.LinkedList; +import java.util.List; +import org.littletonrobotics.junction.Logger; + +public class Vision extends SubsystemBase { + private final VisionConsumer consumer; + private final CameraIO[] io; + private final CameraIOInputsAutoLogged[] inputs; + private final Alert[] disconnectedAlerts; + + public Vision(VisionConsumer consumer, CameraIO... io) { + this.consumer = consumer; + this.io = io; + + // Initialize inputs + this.inputs = new CameraIOInputsAutoLogged[io.length]; + for (int i = 0; i < inputs.length; i++) { + inputs[i] = new CameraIOInputsAutoLogged(); + } + + // Initialize disconnected alerts + this.disconnectedAlerts = new Alert[io.length]; + for (int i = 0; i < inputs.length; i++) { + disconnectedAlerts[i] = + new Alert( + "Vision camera " + Integer.toString(i) + " is disconnected.", AlertType.kWarning); + } + } + + /** + * Returns the X angle to the best target, which can be used for simple servoing with vision. + * + * @param cameraIndex The index of the camera to use. + */ + public Rotation2d getTargetX(int cameraIndex) { + return inputs[cameraIndex].latestTargetObservation.tx(); + } + + @Override + public void periodic() { + for (int i = 0; i < io.length; i++) { + io[i].updateInputs(inputs[i]); + Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]); + } + + // Initialize logging values + List allTagPoses = new LinkedList<>(); + List allRobotPoses = new LinkedList<>(); + List allRobotPosesAccepted = new LinkedList<>(); + List allRobotPosesRejected = new LinkedList<>(); + + // Loop over cameras + for (int cameraIndex = 0; cameraIndex < io.length; cameraIndex++) { + // Update disconnected alert + disconnectedAlerts[cameraIndex].set(!inputs[cameraIndex].connected); + + // Initialize logging values + List tagPoses = new LinkedList<>(); + List robotPoses = new LinkedList<>(); + List robotPosesAccepted = new LinkedList<>(); + List robotPosesRejected = new LinkedList<>(); + + // Add tag poses + for (int tagId : inputs[cameraIndex].tagIds) { + var tagPose = aprilTagLayout.getTagPose(tagId); + if (tagPose.isPresent()) { + tagPoses.add(tagPose.get()); + } + } + + // Loop over pose observations + for (var observation : inputs[cameraIndex].poseObservations) { + // Check whether to reject pose + boolean rejectPose = + observation.tagCount() == 0 // Must have at least one tag + || (observation.tagCount() == 1 + && observation.ambiguity() > maxAmbiguity) // Cannot be high ambiguity + || Math.abs(observation.pose().getZ()) + > maxZError // Must have realistic Z coordinate + + // Must be within the field boundaries + || observation.pose().getX() < 0.0 + || observation.pose().getX() > aprilTagLayout.getFieldLength() + || observation.pose().getY() < 0.0 + || observation.pose().getY() > aprilTagLayout.getFieldWidth(); + + // Add pose to log + robotPoses.add(observation.pose()); + if (rejectPose) { + robotPosesRejected.add(observation.pose()); + } else { + robotPosesAccepted.add(observation.pose()); + } + + // Skip if rejected + if (rejectPose) { + continue; + } + + // Calculate standard deviations + double stdDevFactor = + Math.pow(observation.averageTagDistance(), 2.0) / observation.tagCount(); + double linearStdDev = linearStdDevBaseline * stdDevFactor; + double angularStdDev = angularStdDevBaseline * stdDevFactor; + if (observation.type() == PoseObservationType.MEGATAG_2) { + linearStdDev *= linearStdDevMegatag2Factor; + angularStdDev *= angularStdDevMegatag2Factor; + } + if (cameraIndex < cameraStdDevFactors.length) { + linearStdDev *= cameraStdDevFactors[cameraIndex]; + angularStdDev *= cameraStdDevFactors[cameraIndex]; + } + + // Send vision observation + consumer.accept( + observation.pose().toPose2d(), + observation.timestamp(), + VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev)); + } + + // Log camera metadata + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/TagPoses", + tagPoses.toArray(new Pose3d[0])); + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPoses", + robotPoses.toArray(new Pose3d[0])); + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesAccepted", + robotPosesAccepted.toArray(new Pose3d[0])); + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesRejected", + robotPosesRejected.toArray(new Pose3d[0])); + allTagPoses.addAll(tagPoses); + allRobotPoses.addAll(robotPoses); + allRobotPosesAccepted.addAll(robotPosesAccepted); + allRobotPosesRejected.addAll(robotPosesRejected); + } + + // Log summary data + Logger.recordOutput("Vision/Summary/TagPoses", allTagPoses.toArray(new Pose3d[0])); + Logger.recordOutput("Vision/Summary/RobotPoses", allRobotPoses.toArray(new Pose3d[0])); + Logger.recordOutput( + "Vision/Summary/RobotPosesAccepted", allRobotPosesAccepted.toArray(new Pose3d[0])); + Logger.recordOutput( + "Vision/Summary/RobotPosesRejected", allRobotPosesRejected.toArray(new Pose3d[0])); + } + + @FunctionalInterface + public static interface VisionConsumer { + public void accept( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java new file mode 100644 index 0000000..54757d4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -0,0 +1,52 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.vision; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; + +public class VisionConstants { + // AprilTag layout + public static AprilTagFieldLayout aprilTagLayout = + AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + + // Camera names, must match names configured on coprocessor + public static String camera0Name = "camera_0"; + public static String camera1Name = "camera_1"; + + // Robot to camera transforms + // (Not used by Limelight, configure in web UI instead) + public static Transform3d robotToCamera0 = + new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); + public static Transform3d robotToCamera1 = + new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); + + // Basic filtering thresholds + public static double maxAmbiguity = 0.3; + public static double maxZError = 0.75; + + // Standard deviation baselines, for 1 meter distance and 1 tag + // (Adjusted automatically based on distance and # of tags) + public static double linearStdDevBaseline = 0.02; // Meters + public static double angularStdDevBaseline = 0.06; // Radians + + // Standard deviation multipliers for each camera + // (Adjust to trust some cameras more than others) + public static double[] cameraStdDevFactors = + new double[] { + 1.0, // Camera 0 + 1.0 // Camera 1 + }; + + // Multipliers to apply for MegaTag 2 observations + public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve + public static double angularStdDevMegatag2Factor = + Double.POSITIVE_INFINITY; // No rotation data available +} diff --git a/src/main/java/frc/robot/util/LimelightHelpers.java b/src/main/java/frc/robot/util/LimelightHelpers.java new file mode 100644 index 0000000..d301135 --- /dev/null +++ b/src/main/java/frc/robot/util/LimelightHelpers.java @@ -0,0 +1,1692 @@ +// LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER) + +package frc.robot.util; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; +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.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.DoubleArrayEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; +import frc.robot.util.LimelightHelpers.LimelightResults; +import frc.robot.util.LimelightHelpers.PoseEstimate; +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.Map; +import java.util.concurrent.CompletableFuture; +import java.util.concurrent.ConcurrentHashMap; + +/** + * LimelightHelpers provides static methods and classes for interfacing with Limelight vision + * cameras in FRC. This library supports all Limelight features including AprilTag tracking, Neural + * Networks, and standard color/retroreflective tracking. + */ +public class LimelightHelpers { + + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); + + /** Represents a Color/Retroreflective Target Result extracted from JSON Output */ + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() { + return toPose3D(cameraPose_TargetSpace); + } + + public Pose3d getRobotPose_FieldSpace() { + return toPose3D(robotPose_FieldSpace); + } + + public Pose3d getRobotPose_TargetSpace() { + return toPose3D(robotPose_TargetSpace); + } + + public Pose3d getTargetPose_CameraSpace() { + return toPose3D(targetPose_CameraSpace); + } + + public Pose3d getTargetPose_RobotSpace() { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() { + return toPose2D(cameraPose_TargetSpace); + } + + public Pose2d getRobotPose_FieldSpace2D() { + return toPose2D(robotPose_FieldSpace); + } + + public Pose2d getRobotPose_TargetSpace2D() { + return toPose2D(robotPose_TargetSpace); + } + + public Pose2d getTargetPose_CameraSpace2D() { + return toPose2D(targetPose_CameraSpace); + } + + public Pose2d getTargetPose_RobotSpace2D() { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + /** Represents an AprilTag/Fiducial Target Result extracted from JSON Output */ + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() { + return toPose3D(cameraPose_TargetSpace); + } + + public Pose3d getRobotPose_FieldSpace() { + return toPose3D(robotPose_FieldSpace); + } + + public Pose3d getRobotPose_TargetSpace() { + return toPose3D(robotPose_TargetSpace); + } + + public Pose3d getTargetPose_CameraSpace() { + return toPose3D(targetPose_CameraSpace); + } + + public Pose3d getTargetPose_RobotSpace() { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() { + return toPose2D(cameraPose_TargetSpace); + } + + public Pose2d getRobotPose_FieldSpace2D() { + return toPose2D(robotPose_FieldSpace); + } + + public Pose2d getRobotPose_TargetSpace2D() { + return toPose2D(robotPose_TargetSpace); + } + + public Pose2d getTargetPose_CameraSpace2D() { + return toPose2D(targetPose_CameraSpace); + } + + public Pose2d getTargetPose_RobotSpace2D() { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + /** Represents a Barcode Target Result extracted from JSON Output */ + public static class LimelightTarget_Barcode { + + /** Barcode family type (e.g. "QR", "DataMatrix", etc.) */ + @JsonProperty("fam") + public String family; + + /** Gets the decoded data content of the barcode */ + @JsonProperty("data") + public String data; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("pts") + public double[][] corners; + + public LimelightTarget_Barcode() {} + + public String getFamily() { + return family; + } + } + + /** Represents a Neural Classifier Pipeline Result extracted from JSON Output */ + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() {} + } + + /** Represents a Neural Detector Pipeline Result extracted from JSON Output */ + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + public LimelightTarget_Detector() {} + } + + /** Limelight Results object, parsed from a Limelight's JSON results output. */ + public static class LimelightResults { + + public String error; + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public LimelightResults() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + } + } + + /** Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. */ + public static class RawFiducial { + public int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; + + public RawFiducial( + int id, + double txnc, + double tync, + double ta, + double distToCamera, + double distToRobot, + double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + /** Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. */ + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + public RawDetection( + int classId, + double txnc, + double tync, + double ta, + double corner0_X, + double corner0_Y, + double corner1_X, + double corner1_Y, + double corner2_X, + double corner2_Y, + double corner3_X, + double corner3_Y) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } + } + + /** Represents a 3D Pose Estimate. */ + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + + public RawFiducial[] rawFiducials; + public boolean isMegaTag2; + + /** Instantiates a PoseEstimate object with default values */ + public PoseEstimate() { + this.pose = new Pose2d(); + this.timestampSeconds = 0; + this.latency = 0; + this.tagCount = 0; + this.tagSpan = 0; + this.avgTagDist = 0; + this.avgTagArea = 0; + this.rawFiducials = new RawFiducial[] {}; + this.isMegaTag2 = false; + } + + public PoseEstimate( + Pose2d pose, + double timestampSeconds, + double latency, + int tagCount, + double tagSpan, + double avgTagDist, + double avgTagArea, + RawFiducial[] rawFiducials, + boolean isMegaTag2) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; + this.isMegaTag2 = isMegaTag2; + } + } + + /** Encapsulates the state of an internal Limelight IMU. */ + public static class IMUData { + public double robotYaw = 0.0; + public double Roll = 0.0; + public double Pitch = 0.0; + public double Yaw = 0.0; + public double gyroX = 0.0; + public double gyroY = 0.0; + public double gyroZ = 0.0; + public double accelX = 0.0; + public double accelY = 0.0; + public double accelZ = 0.0; + + public IMUData() {} + + public IMUData(double[] imuData) { + if (imuData != null && imuData.length >= 10) { + this.robotYaw = imuData[0]; + this.Roll = imuData[1]; + this.Pitch = imuData[2]; + this.Yaw = imuData[3]; + this.gyroX = imuData[4]; + this.gyroY = imuData[5]; + this.gyroZ = imuData[6]; + this.accelX = imuData[7]; + this.accelY = imuData[8]; + this.accelZ = imuData[9]; + } + } + } + + private static ObjectMapper mapper; + + /** Print JSON Parse time to the console in milliseconds */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose3d object. Array format: [x, y, z, + * roll, pitch, yaw] where angles are in degrees. + * + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose3d object representing the pose, or empty Pose3d if invalid data + */ + public static Pose3d toPose3D(double[] inData) { + if (inData.length < 6) { + // System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d( + Units.degreesToRadians(inData[3]), + Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose2d object. Uses only x, y, and yaw + * components, ignoring z, roll, and pitch. Array format: [x, y, z, roll, pitch, yaw] where angles + * are in degrees. + * + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose2d object representing the pose, or empty Pose2d if invalid data + */ + public static Pose2d toPose2D(double[] inData) { + if (inData.length < 6) { + // System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + /** + * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * + * @param pose The Pose3d object to convert + * @return A 6-element array containing [x, y, z, roll, pitch, yaw] + */ + public static double[] pose3dToArray(Pose3d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = pose.getTranslation().getZ(); + result[3] = Units.radiansToDegrees(pose.getRotation().getX()); + result[4] = Units.radiansToDegrees(pose.getRotation().getY()); + result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); + return result; + } + + /** + * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. Note: z, roll, and + * pitch will be 0 since Pose2d only contains x, y, and yaw. + * + * @param pose The Pose2d object to convert + * @return A 6-element array containing [x, y, 0, 0, 0, yaw] + */ + public static double[] pose2dToArray(Pose2d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = 0; + result[3] = Units.radiansToDegrees(0); + result[4] = Units.radiansToDegrees(0); + result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); + return result; + } + + private static double extractArrayEntry(double[] inData, int position) { + if (inData.length < position + 1) { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate( + String limelightName, String entryName, boolean isMegaTag2) { + DoubleArrayEntry poseEntry = + LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); + + TimestampedDoubleArray tsValue = poseEntry.getAtomic(); + double[] poseArray = tsValue.value; + long timestamp = tsValue.timestamp; + + if (poseArray.length == 0) { + // Handle the case where no data is available + return null; // or some default PoseEstimate + } + + var pose = toPose2D(poseArray); + double latency = extractArrayEntry(poseArray, 6); + int tagCount = (int) extractArrayEntry(poseArray, 7); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); + + // Convert server timestamp from microseconds to seconds and adjust for latency + double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for (int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int) poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate( + pose, + adjustedTimestamp, + latency, + tagCount, + tagSpan, + tagDist, + tagArea, + rawFiducials, + isMegaTag2); + } + + /** + * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawFiducial objects containing detection details + */ + public static RawFiducial[] getRawFiducials(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); + var rawFiducialArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 7; + if (rawFiducialArray.length % valsPerEntry != 0) { + return new RawFiducial[0]; + } + + int numFiducials = rawFiducialArray.length / valsPerEntry; + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; + + for (int i = 0; i < numFiducials; i++) { + int baseIndex = i * valsPerEntry; + int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; + } + + /** + * Gets the latest raw neural detector results from NetworkTables + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawDetection objects containing detection details + */ + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 12; + if (rawDetectionArray.length % valsPerEntry != 0) { + return new RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + int baseIndex = i * valsPerEntry; // Starting index for this detection's data + int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections[i] = + new RawDetection( + classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, + corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; + } + + /** + * Prints detailed information about a PoseEstimate to standard output. Includes timestamp, + * latency, tag count, tag span, average tag distance, average tag area, and detailed information + * about each detected fiducial. + * + * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." + */ + public static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + + public static Boolean validPoseEstimate(PoseEstimate pose) { + return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static void Flush() { + NetworkTableInstance.getDefault().flush(); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { + String key = tableName + "/" + entryName; + return doubleArrayEntries.computeIfAbsent( + key, + k -> { + NetworkTable table = getLimelightNTTable(tableName); + return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); + }); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static String[] getLimelightNTStringArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + } + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// + + /** + * Does the Limelight have a valid target? + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return True if a valid target is present, false otherwise + */ + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + /** + * Gets the horizontal offset from the crosshair to the target in degrees. + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + /** + * Gets the vertical offset from the crosshair to the target in degrees. + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + /** + * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the + * most accurate 2d metric if you are using a calibrated camera and you don't need adjustable + * crosshair functionality. + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTXNC(String limelightName) { + return getLimelightNTDouble(limelightName, "txnc"); + } + + /** + * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the + * most accurate 2d metric if you are using a calibrated camera and you don't need adjustable + * crosshair functionality. + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTYNC(String limelightName) { + return getLimelightNTDouble(limelightName, "tync"); + } + + /** + * Gets the target area as a percentage of the image (0-100%). + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return Target area percentage (0-100) + */ + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + /** + * T2D is an array that contains several targeting metrcis + * + * @param limelightName Name of the Limelight camera + * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, + * txnc, tync, ta, tid, targetClassIndexDetector, targetClassIndexClassifier, + * targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, + * targetVerticalExtentPixels, targetSkewDegrees] + */ + public static double[] getT2DArray(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "t2d"); + } + + /** + * Gets the number of targets currently detected. + * + * @param limelightName Name of the Limelight camera + * @return Number of detected targets + */ + public static int getTargetCount(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if (t2d.length == 17) { + return (int) t2d[1]; + } + return 0; + } + + /** + * Gets the classifier class index from the currently running neural classifier pipeline + * + * @param limelightName Name of the Limelight camera + * @return Class index from classifier pipeline + */ + public static int getClassifierClassIndex(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if (t2d.length == 17) { + return (int) t2d[10]; + } + return 0; + } + + /** + * Gets the detector class index from the primary result of the currently running neural detector + * pipeline. + * + * @param limelightName Name of the Limelight camera + * @return Class index from detector pipeline + */ + public static int getDetectorClassIndex(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if (t2d.length == 17) { + return (int) t2d[11]; + } + return 0; + } + + /** + * Gets the current neural classifier result class name. + * + * @param limelightName Name of the Limelight camera + * @return Class name string from classifier pipeline + */ + public static String getClassifierClass(String limelightName) { + return getLimelightNTString(limelightName, "tcclass"); + } + + /** + * Gets the primary neural detector result class name. + * + * @param limelightName Name of the Limelight camera + * @return Class name string from detector pipeline + */ + public static String getDetectorClass(String limelightName) { + return getLimelightNTString(limelightName, "tdclass"); + } + + /** + * Gets the pipeline's processing latency contribution. + * + * @param limelightName Name of the Limelight camera + * @return Pipeline latency in milliseconds + */ + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + /** + * Gets the capture latency. + * + * @param limelightName Name of the Limelight camera + * @return Capture latency in milliseconds + */ + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + /** + * Gets the active pipeline index. + * + * @param limelightName Name of the Limelight camera + * @return Current pipeline index (0-9) + */ + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + /** + * Gets the current pipeline type. + * + * @param limelightName Name of the Limelight camera + * @return Pipeline type string (e.g. "retro", "apriltag", etc) + */ + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); + } + + /** + * Gets the full JSON results dump. + * + * @param limelightName Name of the Limelight camera + * @return JSON string containing all current results + */ + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + /** + * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Red Alliance field + * space + */ + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + /** + * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Blue Alliance field + * space + */ + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + /** + * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation relative to the target + */ + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the target + */ + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the camera's coordinate system. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the camera + */ + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the robot's coordinate system. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the robot + */ + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the robot's coordinate system. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the robot + */ + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); + } + + /** + * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) in the WPILib Blue alliance coordinate system. Make sure you are calling + * setRobotOrientation() before calling this method. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when + * you are on the RED alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired", false); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when + * you are on the RED alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + } + + /** + * Gets the current IMU data from NetworkTables. IMU data is formatted as [robotYaw, Roll, Pitch, + * Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. Returns all zeros if data is invalid or + * unavailable. + * + * @param limelightName Name/identifier of the Limelight + * @return IMUData object containing all current IMU data + */ + public static IMUData getIMUData(String limelightName) { + double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); + if (imuData == null || imuData.length < 10) { + return new IMUData(); // Returns object with all zeros + } + return new IMUData(imuData); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * Sets LED mode to be controlled by the current pipeline. + * + * @param limelightName Name of the Limelight camera + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + /** + * Enables standard side-by-side stream mode. + * + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + /** + * Enables Picture-in-Picture mode with secondary stream in the corner. + * + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + /** + * Enables Picture-in-Picture mode with primary stream in the corner. + * + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + /** + * Sets the crop window for the camera. The crop window in the UI must be completely open. + * + * @param limelightName Name of the Limelight camera + * @param cropXMin Minimum X value (-1 to 1) + * @param cropXMax Maximum X value (-1 to 1) + * @param cropYMin Minimum Y value (-1 to 1) + * @param cropYMax Maximum Y value (-1 to 1) + */ + public static void setCropWindow( + String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + /** Sets 3D offset point for easy 3D targeting. */ + public static void setFiducial3DOffset( + String limelightName, double offsetX, double offsetY, double offsetZ) { + double[] entries = new double[3]; + entries[0] = offsetX; + entries[1] = offsetY; + entries[2] = offsetZ; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Sets robot orientation values used by MegaTag2 localization algorithm. + * + * @param limelightName Name/identifier of the Limelight + * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC + * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second + * @param pitch (Unnecessary) Robot pitch in degrees + * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second + * @param roll (Unnecessary) Robot roll in degrees + * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second + */ + public static void SetRobotOrientation( + String limelightName, + double yaw, + double yawRate, + double pitch, + double pitchRate, + double roll, + double rollRate) { + SetRobotOrientation_INTERNAL( + limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); + } + + public static void SetRobotOrientation_NoFlush( + String limelightName, + double yaw, + double yawRate, + double pitch, + double pitchRate, + double roll, + double rollRate) { + SetRobotOrientation_INTERNAL( + limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); + } + + private static void SetRobotOrientation_INTERNAL( + String limelightName, + double yaw, + double yawRate, + double pitch, + double pitchRate, + double roll, + double rollRate, + boolean flush) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + if (flush) { + Flush(); + } + } + + /** + * Configures the IMU mode for MegaTag2 Localization + * + * @param limelightName Name/identifier of the Limelight + * @param mode IMU mode. + */ + public static void SetIMUMode(String limelightName, int mode) { + setLimelightNTDouble(limelightName, "imumode_set", mode); + } + + /** + * Sets the 3D point-of-interest offset for the current fiducial pipeline. + * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking + * + * @param limelightName Name/identifier of the Limelight + * @param x X offset in meters + * @param y Y offset in meters + * @param z Z offset in meters + */ + public static void SetFidcuial3DOffset(String limelightName, double x, double y, double z) { + + double[] entries = new double[3]; + entries[0] = x; + entries[1] = y; + entries[2] = z; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Overrides the valid AprilTag IDs that will be used for localization. Tags not in this list will + * be ignored for robot pose estimation. + * + * @param limelightName Name/identifier of the Limelight + * @param validIDs Array of valid AprilTag IDs to track + */ + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + /** + * Sets the downscaling factor for AprilTag detection. Increasing downscale can improve + * performance at the cost of potentially reduced detection range. + * + * @param limelightName Name/identifier of the Limelight + * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to + * 0 for pipeline control. + */ + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) { + int d = 0; // pipeline + if (downscale == 1.0) { + d = 1; + } + if (downscale == 1.5) { + d = 2; + } + if (downscale == 2) { + d = 3; + } + if (downscale == 3) { + d = 4; + } + if (downscale == 4) { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + + /** + * Sets the camera pose relative to the robot. + * + * @param limelightName Name of the Limelight camera + * @param forward Forward offset in meters + * @param side Side offset in meters + * @param up Up offset in meters + * @param roll Roll angle in degrees + * @param pitch Pitch angle in degrees + * @param yaw Yaw angle in degrees + */ + public static void setCameraPose_RobotSpace( + String limelightName, + double forward, + double side, + double up, + double roll, + double pitch, + double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** Asynchronously take snapshot. */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync( + () -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Gets the latest JSON results output and returns a LimelightResults object. + * + * @param limelightName Name of the Limelight camera + * @return LimelightResults object containing all current target data + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = + new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..7508481 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2026.1.1-rc-3", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2026", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2026.1.1-rc-3", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2026.1.1-rc-3", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2026.1.1-rc-3", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2026.1.1-rc-3" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2026.1.1-rc-3" + } + ] +} From c2d39e673984b3fac6a61362ebd501e61d861e9b Mon Sep 17 00:00:00 2001 From: Maxwell Morgan Date: Mon, 19 Jan 2026 13:55:53 -0500 Subject: [PATCH 05/61] Move VisionConstants to Constants.java --- .vscode/settings.json | 3 +- src/main/java/frc/robot/Constants.java | 43 ++++++++++++++- .../vision/CameraIOPhotonVision.java | 2 +- .../vision/CameraIOPhotonVisionSim.java | 4 +- .../frc/robot/subsystems/vision/Vision.java | 2 +- .../subsystems/vision/VisionConstants.java | 52 ------------------- .../java/frc/robot/util/LimelightHelpers.java | 2 - 7 files changed, 48 insertions(+), 60 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/vision/VisionConstants.java diff --git a/.vscode/settings.json b/.vscode/settings.json index a42e75d..2290187 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -69,5 +69,6 @@ }, "[java]": { "editor.defaultFormatter": "redhat.java" - } + }, + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c14e9a1..f945980 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -9,7 +9,11 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.util.Units; @@ -165,5 +169,42 @@ public static record ModuleConfig( 4, 4.0); } - public static final class VisionConstants {} + public class VisionConstants { + // AprilTag layout + public static AprilTagFieldLayout aprilTagLayout = + AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + + // Camera names, must match names configured on coprocessor + public static String camera0Name = "camera_0"; + public static String camera1Name = "camera_1"; + + // Robot to camera transforms + // (Not used by Limelight, configure in web UI instead) + public static Transform3d robotToCamera0 = + new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); + public static Transform3d robotToCamera1 = + new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); + + // Basic filtering thresholds + public static double maxAmbiguity = 0.3; + public static double maxZError = 0.75; + + // Standard deviation baselines, for 1 meter distance and 1 tag + // (Adjusted automatically based on distance and # of tags) + public static double linearStdDevBaseline = 0.02; // Meters + public static double angularStdDevBaseline = 0.06; // Radians + + // Standard deviation multipliers for each camera + // (Adjust to trust some cameras more than others) + public static double[] cameraStdDevFactors = + new double[] { + 1.0, // Camera 0 + 1.0 // Camera 1 + }; + + // Multipliers to apply for MegaTag 2 observations + public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve + public static double angularStdDevMegatag2Factor = + Double.POSITIVE_INFINITY; // No rotation data available + } } diff --git a/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVision.java index 0ad84c2..db5b2a3 100644 --- a/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVision.java @@ -7,7 +7,7 @@ package frc.robot.subsystems.vision; -import static frc.robot.subsystems.vision.VisionConstants.*; +import static frc.robot.Constants.VisionConstants.*; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVisionSim.java index 56e41da..c16900d 100644 --- a/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVisionSim.java +++ b/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVisionSim.java @@ -7,7 +7,7 @@ package frc.robot.subsystems.vision; -import static frc.robot.subsystems.vision.VisionConstants.aprilTagLayout; +import static frc.robot.Constants.VisionConstants.aprilTagLayout; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform3d; @@ -51,4 +51,4 @@ public void updateInputs(CameraIOInputs inputs) { visionSim.update(poseSupplier.get()); super.updateInputs(inputs); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 7b9e651..3d3192a 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -7,7 +7,7 @@ package frc.robot.subsystems.vision; -import static frc.robot.subsystems.vision.VisionConstants.*; +import static frc.robot.Constants.VisionConstants.*; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java deleted file mode 100644 index 54757d4..0000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright (c) 2021-2026 Littleton Robotics -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.vision; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; - -public class VisionConstants { - // AprilTag layout - public static AprilTagFieldLayout aprilTagLayout = - AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); - - // Camera names, must match names configured on coprocessor - public static String camera0Name = "camera_0"; - public static String camera1Name = "camera_1"; - - // Robot to camera transforms - // (Not used by Limelight, configure in web UI instead) - public static Transform3d robotToCamera0 = - new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); - public static Transform3d robotToCamera1 = - new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); - - // Basic filtering thresholds - public static double maxAmbiguity = 0.3; - public static double maxZError = 0.75; - - // Standard deviation baselines, for 1 meter distance and 1 tag - // (Adjusted automatically based on distance and # of tags) - public static double linearStdDevBaseline = 0.02; // Meters - public static double angularStdDevBaseline = 0.06; // Radians - - // Standard deviation multipliers for each camera - // (Adjust to trust some cameras more than others) - public static double[] cameraStdDevFactors = - new double[] { - 1.0, // Camera 0 - 1.0 // Camera 1 - }; - - // Multipliers to apply for MegaTag 2 observations - public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve - public static double angularStdDevMegatag2Factor = - Double.POSITIVE_INFINITY; // No rotation data available -} diff --git a/src/main/java/frc/robot/util/LimelightHelpers.java b/src/main/java/frc/robot/util/LimelightHelpers.java index d301135..3c5a931 100644 --- a/src/main/java/frc/robot/util/LimelightHelpers.java +++ b/src/main/java/frc/robot/util/LimelightHelpers.java @@ -20,8 +20,6 @@ import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.TimestampedDoubleArray; -import frc.robot.util.LimelightHelpers.LimelightResults; -import frc.robot.util.LimelightHelpers.PoseEstimate; import java.io.IOException; import java.net.HttpURLConnection; import java.net.MalformedURLException; From 7ffb5631e60b9520b25522ed2cb9f1a98b67bc26 Mon Sep 17 00:00:00 2001 From: Maxwell Morgan Date: Thu, 22 Jan 2026 17:44:46 -0500 Subject: [PATCH 06/61] Add vision to RobotContainer --- src/main/java/frc/robot/RobotContainer.java | 37 +++++++++++++++------ src/main/java/frc/robot/RobotState.java | 2 +- 2 files changed, 27 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5d5bafb..af28aa7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,25 +15,41 @@ import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.SwerveMod; import frc.robot.subsystems.drive.SwerveMod.ModuleName; +import frc.robot.subsystems.vision.Vision; public class RobotContainer { private final XboxController driver = new XboxController(0); private final Drive drive; + private final Vision vision; public RobotContainer() { - if (Robot.isReal()) { - drive = new Drive(null, null); - } else { - drive = - new Drive( - new SwerveMod[] { + + switch (Constants.kCurrentMode) { + case REAL: + // Initialize real IO implementations. + drive = new Drive(null, null); + vision = new Vision(null); + break; + case SIM: + // Initialize simulation IO implementations. + drive = new Drive( + new SwerveMod[] { new SwerveMod(new ModuleIOSim(), ModuleName.FRONT_LEFT), new SwerveMod(new ModuleIOSim(), ModuleName.FRONT_RIGHT), new SwerveMod(new ModuleIOSim(), ModuleName.BACK_LEFT), new SwerveMod(new ModuleIOSim(), ModuleName.BACK_RIGHT), - }, - new GyroIO() {}); + }, + new GyroIO() { + }); + vision = new Vision(null); + break; + case REPLAY: + default: + // Initialize default IO implementations. + drive = new Drive(null, null); + vision = new Vision(null, null); + break; } configureBindings(); @@ -50,9 +66,8 @@ private void configureBindings() { } public void robotPeriodic() { - OdometryObservation obs = - new OdometryObservation( - Timer.getTimestamp(), drive.getModulePositions(), drive.getRawGyroRotation()); + OdometryObservation obs = new OdometryObservation( + Timer.getTimestamp(), drive.getModulePositions(), drive.getRawGyroRotation()); RobotState.getInstance().addOdometryObservation(obs); } diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index bf1b89e..4463a77 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -12,7 +12,7 @@ import org.littletonrobotics.junction.Logger; public class RobotState { - private static RobotState instance = new RobotState(); + private static RobotState instance; public static RobotState getInstance() { if (instance == null) instance = new RobotState(); From 9ab4a4ec8a5ea0f6c278f9fe676b90cc2c478a1a Mon Sep 17 00:00:00 2001 From: Maxwell Morgan Date: Fri, 23 Jan 2026 17:01:08 -0500 Subject: [PATCH 07/61] Add FieldConstants and AdvantageKit swerve template --- src/main/deploy/.gitkeep | 0 src/main/java/frc/robot/Constants.java | 416 +++++++++++++++++- src/main/java/frc/robot/RobotContainer.java | 96 +++- src/main/java/frc/robot/RobotState.java | 8 +- .../frc/robot/commands/DriveCommands.java | 310 ++++++++++++- .../frc/robot/subsystems/drive/Drive.java | 301 +++++++++---- .../frc/robot/subsystems/drive/GyroIO.java | 30 +- .../robot/subsystems/drive/GyroIONavX.java | 44 ++ .../robot/subsystems/drive/GyroIOPigeon2.java | 62 +++ .../frc/robot/subsystems/drive/Module.java | 141 ++++++ .../frc/robot/subsystems/drive/ModuleIO.java | 86 ++-- .../robot/subsystems/drive/ModuleIOSim.java | 138 +++--- .../subsystems/drive/ModuleIOTalonFX.java | 265 +++++++++++ .../subsystems/drive/ModuleIOTalonFXS.java | 252 +++++++++++ .../drive/PhoenixOdometryThread.java | 159 +++++++ .../frc/robot/subsystems/drive/SwerveMod.java | 108 ----- .../java/frc/robot/util/AllianceFlipUtil.java | 55 +++ .../java/frc/robot/util/FieldConstants.java | 339 ++++++++++++++ .../java/frc/robot/util/LocalADStarAK.java | 160 +++++++ src/main/java/frc/robot/util/PhoenixUtil.java | 21 + vendordeps/Studica.json | 71 +++ 21 files changed, 2679 insertions(+), 383 deletions(-) delete mode 100644 src/main/deploy/.gitkeep create mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIONavX.java create mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java create mode 100644 src/main/java/frc/robot/subsystems/drive/Module.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java create mode 100644 src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/SwerveMod.java create mode 100644 src/main/java/frc/robot/util/AllianceFlipUtil.java create mode 100644 src/main/java/frc/robot/util/FieldConstants.java create mode 100644 src/main/java/frc/robot/util/LocalADStarAK.java create mode 100644 src/main/java/frc/robot/util/PhoenixUtil.java create mode 100644 vendordeps/Studica.json diff --git a/src/main/deploy/.gitkeep b/src/main/deploy/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 60a7e4d..86c0f6c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,12 +7,48 @@ package frc.robot; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; +import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; +import com.pathplanner.lib.config.ModuleConfig; +import com.pathplanner.lib.config.RobotConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.util.Units.*; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.MomentOfInertia; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotBase; import java.util.Map; @@ -38,29 +74,59 @@ public static enum Mode { REPLAY } - public class DriveConstants { - - public static class ModuleConfigs { - - public static record ModuleConfig( - int driveMotorID, int angleMotorID, int canCoderID, Rotation2d angleOffset) {} - - /** Module 0 (front left) configs. */ - public static final ModuleConfig FrontLeft = - new ModuleConfig(1, 2, 19, Rotation2d.fromDegrees(304.36523 - 180)); + public static boolean kDisableHAL = false; - /** Module 1 (front right) configs. */ - public static final ModuleConfig FrontRight = - new ModuleConfig(2, 4, 20, Rotation2d.fromDegrees(206.455)); + public static void disableHAL() { + kDisableHAL = true; + } - /** Module 2 (back left) configs. */ - public static final ModuleConfig BackLeft = - new ModuleConfig(5, 6, 21, Rotation2d.fromDegrees(35.419922 + 180)); + public class DriveConstants { - /** Module 3 (back right) configs. */ - public static final ModuleConfig BackRight = - new ModuleConfig(7, 8, 22, Rotation2d.fromDegrees(116.89453)); - } + // TunerConstants doesn't include these constants + public static final double kOdometryFrequency = + ModuleConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0; + public static final double kDriveBaseRadius = + Math.max( + Math.max( + Math.hypot( + ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), + Math.hypot( + ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY)), + Math.max( + Math.hypot(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), + Math.hypot( + ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY))); + + public static final Translation2d[] kModuleTranslations = + new Translation2d[] { + new Translation2d( + ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), + new Translation2d( + ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY), + new Translation2d(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), + new Translation2d( + ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY) + }; + + // PathPlanner config constants + public static final double kRobotMassKG = 74.088; + public static final double kRobotMOI = 6.883; + /** Coefficient of friction */ + public static final double kWheelCOF = 1.2; + + public static final RobotConfig kPathplannerConfig = + new RobotConfig( + kRobotMOI, + kRobotMOI, + new ModuleConfig( + ModuleConstants.FrontLeft.WheelRadius, + ModuleConstants.kSpeedAt12Volts.in(MetersPerSecond), + kWheelCOF, + DCMotor.getKrakenX60Foc(1) + .withReduction(ModuleConstants.FrontLeft.DriveMotorGearRatio), + ModuleConstants.FrontLeft.SlipCurrent, + 1), + kModuleTranslations); public static final IdleMode kDriveIdleMode = IdleMode.kBrake; public static final IdleMode kAngleIdleMode = IdleMode.kBrake; @@ -164,4 +230,312 @@ public static record ModuleConfig( 3, 3.0, 4, 4.0); } + + public class ModuleConstants { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with + // the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = + new Slot0Configs() + .withKP(100) + .withKI(0) + .withKD(0.5) + .withKS(0.1) + .withKV(1.91) + .withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = + new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = + DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = + SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to + // RemoteCANcoder + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current kSlipCurrent = Amps.of(120.0); + + // Initial configs for the drive and steer motors and the azimuth encoder; these + // cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API + // documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a + // relatively + // low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true)); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 3.8181818181818183; + + private static final double kDriveGearRatio = 7.363636363636365; + private static final double kSteerGearRatio = 15.42857142857143; + private static final Distance kWheelRadius = Inches.of(2.167); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 1; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = + new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + ConstantCreator = + new SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + // Front Left + private static final int kFrontLeftDriveMotorId = 3; + private static final int kFrontLeftSteerMotorId = 2; + private static final int kFrontLeftEncoderId = 1; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(10); + private static final Distance kFrontLeftYPos = Inches.of(10); + + // Front Right + private static final int kFrontRightDriveMotorId = 1; + private static final int kFrontRightSteerMotorId = 0; + private static final int kFrontRightEncoderId = 0; + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(10); + private static final Distance kFrontRightYPos = Inches.of(-10); + + // Back Left + private static final int kBackLeftDriveMotorId = 7; + private static final int kBackLeftSteerMotorId = 6; + private static final int kBackLeftEncoderId = 3; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); + private static final boolean kBackLeftSteerMotorInverted = true; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-10); + private static final Distance kBackLeftYPos = Inches.of(10); + + // Back Right + private static final int kBackRightDriveMotorId = 5; + private static final int kBackRightSteerMotorId = 4; + private static final int kBackRightEncoderId = 2; + private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-10); + private static final Distance kBackRightYPos = Inches.of(-10); + + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, + kFrontLeftDriveMotorId, + kFrontLeftEncoderId, + kFrontLeftEncoderOffset, + kFrontLeftXPos, + kFrontLeftYPos, + kInvertLeftSide, + kFrontLeftSteerMotorInverted, + kFrontLeftEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, + kFrontRightDriveMotorId, + kFrontRightEncoderId, + kFrontRightEncoderOffset, + kFrontRightXPos, + kFrontRightYPos, + kInvertRightSide, + kFrontRightSteerMotorInverted, + kFrontRightEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, + kBackLeftDriveMotorId, + kBackLeftEncoderId, + kBackLeftEncoderOffset, + kBackLeftXPos, + kBackLeftYPos, + kInvertLeftSide, + kBackLeftSteerMotorInverted, + kBackLeftEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, + kBackRightDriveMotorId, + kBackRightEncoderId, + kBackRightEncoderOffset, + kBackRightXPos, + kBackRightYPos, + kInvertRightSide, + kBackRightSteerMotorInverted, + kBackRightEncoderInverted); + + /** + * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot + * program,. + */ + // public static CommandSwerveDrivetrain createDrivetrain() { + // return new CommandSwerveDrivetrain( + // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); + // } + + /** + * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. + */ + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the + * devices themselves. If they need the devices, they can access them through getters in the + * classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules) { + super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the + * devices themselves. If they need the devices, they can access them through getters in the + * classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or + * set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules) { + super( + TalonFX::new, + TalonFX::new, + CANcoder::new, + drivetrainConstants, + odometryUpdateFrequency, + modules); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the + * devices themselves. If they need the devices, they can access them through getters in the + * classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or + * set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation in the + * form [x, y, theta]áµ€, with units in meters and radians + * @param visionStandardDeviation The standard deviation for vision calculation in the form + * [x, y, theta]áµ€, with units in meters and radians + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { + super( + TalonFX::new, + TalonFX::new, + CANcoder::new, + drivetrainConstants, + odometryUpdateFrequency, + odometryStandardDeviation, + visionStandardDeviation, + modules); + } + } + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5d5bafb..0e9a9a7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,36 +4,64 @@ package frc.robot; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.XboxController; 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.ModuleConstants; import frc.robot.RobotState.OdometryObservation; import frc.robot.commands.DriveCommands; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.GyroIO; +import frc.robot.subsystems.drive.GyroIOPigeon2; +import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; -import frc.robot.subsystems.drive.SwerveMod; -import frc.robot.subsystems.drive.SwerveMod.ModuleName; +import frc.robot.subsystems.drive.ModuleIOTalonFX; +import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.FieldConstants; +import frc.robot.util.FieldConstants.Hub; public class RobotContainer { - private final XboxController driver = new XboxController(0); + private final CommandXboxController driver = new CommandXboxController(0); private final Drive drive; public RobotContainer() { - if (Robot.isReal()) { - drive = new Drive(null, null); - } else { - drive = - new Drive( - new SwerveMod[] { - new SwerveMod(new ModuleIOSim(), ModuleName.FRONT_LEFT), - new SwerveMod(new ModuleIOSim(), ModuleName.FRONT_RIGHT), - new SwerveMod(new ModuleIOSim(), ModuleName.BACK_LEFT), - new SwerveMod(new ModuleIOSim(), ModuleName.BACK_RIGHT), - }, - new GyroIO() {}); + switch (Constants.kCurrentMode) { + case REAL: + drive = new Drive( + new GyroIOPigeon2(), + new ModuleIOTalonFX(ModuleConstants.FrontLeft), + new ModuleIOTalonFX(ModuleConstants.FrontRight), + new ModuleIOTalonFX(ModuleConstants.BackLeft), + new ModuleIOTalonFX(ModuleConstants.BackRight)); + break; + case SIM: + drive = new Drive( + new GyroIO() { + }, + new ModuleIOSim(ModuleConstants.FrontLeft), + new ModuleIOSim(ModuleConstants.FrontRight), + new ModuleIOSim(ModuleConstants.BackLeft), + new ModuleIOSim(ModuleConstants.BackRight)); + break; + case REPLAY: + default: + drive = new Drive( + new GyroIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }); + break; } configureBindings(); @@ -42,17 +70,37 @@ public RobotContainer() { private void configureBindings() { drive.setDefaultCommand( DriveCommands.joystickDrive( - drive, - () -> -driver.getLeftY(), - () -> -driver.getLeftX(), - () -> -driver.getRightX(), - () -> false)); + drive, () -> -driver.getLeftY(), () -> -driver.getLeftX(), () -> -driver.getRightX())); + + driver + .rightBumper() + .whileTrue( + DriveCommands.joystickDriveAtAngle( + drive, + () -> -driver.getLeftY(), // xSupplier + () -> -driver.getLeftX(), // ySupplier + () -> { + Pose2d robotPose = RobotState.getInstance().getEstimatedPose(); + Translation2d target = AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d()); + + Translation2d delta = target.minus(robotPose.getTranslation()); + + return new Rotation2d(Math.atan2(delta.getY(), delta.getX())) + .plus(Rotation2d.k180deg); // Because KitBot shooter is on the back + })); + + driver + .y() + .onTrue( + DriveCommands.turnToPoint( + drive, + () -> RobotState.getInstance().getEstimatedPose(), + () -> Hub.innerCenterPoint.toTranslation2d())); } public void robotPeriodic() { - OdometryObservation obs = - new OdometryObservation( - Timer.getTimestamp(), drive.getModulePositions(), drive.getRawGyroRotation()); + OdometryObservation obs = new OdometryObservation( + Timer.getTimestamp(), drive.getModulePositions(), drive.getRawGyroRotation()); RobotState.getInstance().addOdometryObservation(obs); } diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index bf1b89e..9a2ee63 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -58,11 +58,9 @@ public void addVisionMeasurement(VisionMeasurement measurement) { } /** Reset pose estimate and align gyro frame to the given pose. */ - public void resetPose(Pose2d pose) { - - poseEstimator.resetPose(pose); - - Logger.recordOutput("RobotState/EstimatedPose", poseEstimator.getEstimatedPosition()); + public void setPose( + Pose2d pose, SwerveModulePosition[] modulePositions, Rotation2d rawGyroRotation) { + poseEstimator.resetPosition(rawGyroRotation, modulePositions, pose); } /** Field-relative estimated robot pose. */ diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 303ec45..613b470 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -1,42 +1,316 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + package frc.robot.commands; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.Constants.DriveConstants; import frc.robot.subsystems.drive.Drive; -import java.util.function.BooleanSupplier; +import frc.robot.util.AllianceFlipUtil; +import java.text.DecimalFormat; +import java.text.NumberFormat; +import java.util.LinkedList; +import java.util.List; import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class DriveCommands { + private static final double DEADBAND = 0.1; + private static final double ANGLE_KP = 7.0; + private static final double ANGLE_KD = 0.3; + private static final double ANGLE_MAX_VELOCITY = 8.0; + private static final double ANGLE_MAX_ACCELERATION = 20.0; + private static final double FF_START_DELAY = 2.0; // Secs + private static final double FF_RAMP_RATE = 0.1; // Volts/Sec + private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec + private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 + + private DriveCommands() {} -public final class DriveCommands { + private static Translation2d getLinearVelocityFromJoysticks(double x, double y) { + // Apply deadband + double linearMagnitude = MathUtil.applyDeadband(Math.hypot(x, y), DEADBAND); + Rotation2d linearDirection = new Rotation2d(Math.atan2(y, x)); + + // Square magnitude for more precise control + linearMagnitude = linearMagnitude * linearMagnitude; + + // Return new linear velocity + return new Pose2d(Translation2d.kZero, linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, Rotation2d.kZero)) + .getTranslation(); + } - public static final Command joystickDrive( + /** + * Field relative drive command using two joysticks (controlling linear and angular velocities). + */ + public static Command joystickDrive( Drive drive, DoubleSupplier xSupplier, DoubleSupplier ySupplier, - DoubleSupplier rotationSupplier, - BooleanSupplier isRobotRelative) { + DoubleSupplier omegaSupplier) { return Commands.run( () -> { - double x = xSupplier.getAsDouble(); - double y = ySupplier.getAsDouble(); - double rot = rotationSupplier.getAsDouble(); + // Get linear velocity + Translation2d linearVelocity = + getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()); - x = Math.abs(x) > DriveConstants.kDeadband ? x * Math.abs(x) : 0.0; - y = Math.abs(y) > DriveConstants.kDeadband ? y * Math.abs(y) : 0.0; - rot = Math.abs(rot) > DriveConstants.kDeadband ? rot * Math.abs(rot) : 0.0; + // Apply rotation deadband + double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); - x *= DriveConstants.kPhysicalMaxSpeed; - y *= DriveConstants.kPhysicalMaxSpeed; - rot *= DriveConstants.kMaxTeleAngularSpeed; + // Square rotation value for more precise control + omega = Math.copySign(omega * omega, omega); + // Convert to field relative speeds & send command ChassisSpeeds speeds = - isRobotRelative.getAsBoolean() - ? new ChassisSpeeds(x, y, rot) - : ChassisSpeeds.fromFieldRelativeSpeeds(x, y, rot, drive.getRawGyroRotation()); + new ChassisSpeeds( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), + omega * drive.getMaxAngularSpeedRadPerSec()); - drive.runVelocity(speeds, false); + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + speeds, AllianceFlipUtil.apply(drive.getRawGyroRotation()))); }, drive); } + + /** + * Field relative drive command using joystick for linear control and PID for angular control. + * Possible use cases include snapping to an angle, aiming at a vision target, or controlling + * absolute rotation with a joystick. + */ + public static Command joystickDriveAtAngle( + Drive drive, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + Supplier rotationSupplier) { + + // Create PID controller + ProfiledPIDController angleController = + new ProfiledPIDController( + ANGLE_KP, + 0.0, + ANGLE_KD, + new TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION)); + angleController.enableContinuousInput(-Math.PI, Math.PI); + + // Construct command + return Commands.run( + () -> { + // Get linear velocity + Translation2d linearVelocity = + getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + + // Calculate angular speed + double omega = + angleController.calculate( + drive.getRawGyroRotation().getRadians(), rotationSupplier.get().getRadians()); + + // Convert to field relative speeds & send command + ChassisSpeeds speeds = + new ChassisSpeeds( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), + omega); + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + speeds, AllianceFlipUtil.apply(drive.getRawGyroRotation()))); + }, + drive) + + // Reset PID controller when command starts + .beforeStarting(() -> angleController.reset(drive.getRawGyroRotation().getRadians())); + } + + public static Command turnToPoint( + Drive drive, Supplier robotPoseSupplier, Supplier targetSupplier) { + + // Create PID controller + ProfiledPIDController angleController = + new ProfiledPIDController( + ANGLE_KP, + 0.0, + ANGLE_KD, + new TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION)); + angleController.enableContinuousInput(-Math.PI, Math.PI); + angleController.setTolerance(Math.toRadians(2.0)); + + return Commands.run( + () -> { + Pose2d robotPose = robotPoseSupplier.get(); + + Translation2d target = AllianceFlipUtil.apply(targetSupplier.get()); + + Translation2d delta = target.minus(robotPose.getTranslation()); + + Rotation2d targetAngle = new Rotation2d(Math.atan2(delta.getY(), delta.getX())); + + double omega = + angleController.calculate( + drive.getRawGyroRotation().getRadians(), targetAngle.getRadians()); + + drive.runVelocity(new ChassisSpeeds(0, 0, omega)); + }, + drive) + .until(angleController::atGoal) + .beforeStarting(() -> angleController.reset(drive.getRawGyroRotation().getRadians())); + } + + // ----------------------- Characterization Commands ----------------------- + + /** + * Measures the velocity feedforward constants for the drive motors. + * + *

This command should only be used in voltage control mode. + */ + public static Command feedforwardCharacterization(Drive drive) { + List velocitySamples = new LinkedList<>(); + List voltageSamples = new LinkedList<>(); + Timer timer = new Timer(); + + return Commands.sequence( + // Reset data + Commands.runOnce( + () -> { + velocitySamples.clear(); + voltageSamples.clear(); + }), + + // Allow modules to orient + Commands.run( + () -> { + drive.runCharacterization(0.0); + }, + drive) + .withTimeout(FF_START_DELAY), + + // Start timer + Commands.runOnce(timer::restart), + + // Accelerate and gather data + Commands.run( + () -> { + double voltage = timer.get() * FF_RAMP_RATE; + drive.runCharacterization(voltage); + velocitySamples.add(drive.getFFCharacterizationVelocity()); + voltageSamples.add(voltage); + }, + drive) + + // When cancelled, calculate and print results + .finallyDo( + () -> { + int n = velocitySamples.size(); + double sumX = 0.0; + double sumY = 0.0; + double sumXY = 0.0; + double sumX2 = 0.0; + for (int i = 0; i < n; i++) { + sumX += velocitySamples.get(i); + sumY += voltageSamples.get(i); + sumXY += velocitySamples.get(i) * voltageSamples.get(i); + sumX2 += velocitySamples.get(i) * velocitySamples.get(i); + } + double kS = (sumY * sumX2 - sumX * sumXY) / (n * sumX2 - sumX * sumX); + double kV = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX); + + NumberFormat formatter = new DecimalFormat("#0.00000"); + System.out.println("********** Drive FF Characterization Results **********"); + System.out.println("\tkS: " + formatter.format(kS)); + System.out.println("\tkV: " + formatter.format(kV)); + })); + } + + /** Measures the robot's wheel radius by spinning in a circle. */ + public static Command wheelRadiusCharacterization(Drive drive) { + SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE); + WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); + + return Commands.parallel( + // Drive control sequence + Commands.sequence( + // Reset acceleration limiter + Commands.runOnce( + () -> { + limiter.reset(0.0); + }), + + // Turn in place, accelerating up to full speed + Commands.run( + () -> { + double speed = limiter.calculate(WHEEL_RADIUS_MAX_VELOCITY); + drive.runVelocity(new ChassisSpeeds(0.0, 0.0, speed)); + }, + drive)), + + // Measurement sequence + Commands.sequence( + // Wait for modules to fully orient before starting measurement + Commands.waitSeconds(1.0), + + // Record starting measurement + Commands.runOnce( + () -> { + state.positions = drive.getWheelRadiusCharacterizationPositions(); + state.lastAngle = drive.getRawGyroRotation(); + state.gyroDelta = 0.0; + }), + + // Update gyro delta + Commands.run( + () -> { + var rotation = drive.getRawGyroRotation(); + state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians()); + state.lastAngle = rotation; + }) + + // When cancelled, calculate and print results + .finallyDo( + () -> { + double[] positions = drive.getWheelRadiusCharacterizationPositions(); + double wheelDelta = 0.0; + for (int i = 0; i < 4; i++) { + wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; + } + double wheelRadius = + (state.gyroDelta * DriveConstants.kDriveBaseRadius) / wheelDelta; + + NumberFormat formatter = new DecimalFormat("#0.000"); + System.out.println( + "********** Wheel Radius Characterization Results **********"); + System.out.println( + "\tWheel Delta: " + formatter.format(wheelDelta) + " radians"); + System.out.println( + "\tGyro Delta: " + formatter.format(state.gyroDelta) + " radians"); + System.out.println( + "\tWheel Radius: " + + formatter.format(wheelRadius) + + " meters, " + + formatter.format(Units.metersToInches(wheelRadius)) + + " inches"); + }))); + } + + private static class WheelRadiusCharacterizationState { + double[] positions = new double[4]; + Rotation2d lastAngle = Rotation2d.kZero; + double gyroDelta = 0.0; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 98724bb..d1e58b3 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -1,154 +1,277 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + package frc.robot.subsystems.drive; +import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.hal.FRCNetComm.tInstances; +import edu.wpi.first.hal.FRCNetComm.tResourceType; +import edu.wpi.first.hal.HAL; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants; import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.Mode; +import frc.robot.Constants.ModuleConstants; +import frc.robot.RobotState; +import frc.robot.RobotState.OdometryObservation; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Drive extends SubsystemBase { - private final SwerveMod[] modules; + static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); + private final Module[] modules = new Module[4]; // FL, FR, BL, BR + private final SysIdRoutine sysId; + private final Alert gyroDisconnectedAlert = + new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); - @AutoLogOutput(key = "Drive/WantedStates") - private SwerveModuleState[] wantedStates; + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); + private Rotation2d rawGyroRotation = Rotation2d.kZero; + private SwerveModulePosition[] lastModulePositions = // For delta tracking + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; - @AutoLogOutput(key = "Drive/ActualStates") - private SwerveModuleState[] actualStates; + public Drive( + GyroIO gyroIO, + ModuleIO flModuleIO, + ModuleIO frModuleIO, + ModuleIO blModuleIO, + ModuleIO brModuleIO) { + this.gyroIO = gyroIO; + modules[0] = new Module(flModuleIO, 0, ModuleConstants.FrontLeft); + modules[1] = new Module(frModuleIO, 1, ModuleConstants.FrontRight); + modules[2] = new Module(blModuleIO, 2, ModuleConstants.BackLeft); + modules[3] = new Module(brModuleIO, 3, ModuleConstants.BackRight); - private SwerveModulePosition[] lastModulePositions; + // Usage reporting for swerve template + HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDriveSwerve_AdvantageKit); - private Rotation2d rawGyroRotation = new Rotation2d(); - static final Lock m_odometryLock = new ReentrantLock(); + // Start odometry thread + PhoenixOdometryThread.getInstance().start(); - public Drive(SwerveMod[] modules, GyroIO gyroIO) { - this.modules = modules; - this.gyroIO = gyroIO; - - wantedStates = - new SwerveModuleState[] { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() - }; - - actualStates = - new SwerveModuleState[] { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() - }; - - lastModulePositions = - new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() - }; + // Configure SysId + sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + null, + null, + null, + (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), + new SysIdRoutine.Mechanism( + (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); } @Override public void periodic() { - m_odometryLock.lock(); // Prevents odometry updates while reading data + odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); Logger.processInputs("Drive/Gyro", gyroInputs); - for (SwerveMod mod : modules) { - mod.periodic(); + for (var module : modules) { + module.periodic(); } - actualStates = getModuleStates(); - m_odometryLock.unlock(); + odometryLock.unlock(); + // Stop moving when disabled if (DriverStation.isDisabled()) { - for (SwerveMod mod : modules) { - mod.stop(); + for (var module : modules) { + module.stop(); } } - SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; - - for (int i = 0; i < 4; i++) { - SwerveModulePosition current = modules[i].getPosition(); - moduleDeltas[i] = - new SwerveModulePosition( - current.distanceMeters - lastModulePositions[i].distanceMeters, current.angle); - lastModulePositions[i] = current; + // Log empty setpoint states when disabled + if (DriverStation.isDisabled()) { + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); } - if (gyroInputs.data.connected()) { - rawGyroRotation = gyroInputs.data.yawPosition(); - } else { - Twist2d twist = DriveConstants.swerveKinematics.toTwist2d(moduleDeltas); - rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); + // Update odometry + double[] sampleTimestamps = + modules[0].getOdometryTimestamps(); // All signals are sampled together + int sampleCount = sampleTimestamps.length; + for (int i = 0; i < sampleCount; i++) { + // Read wheel positions and deltas from each module + SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; + moduleDeltas[moduleIndex] = + new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters + - lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle); + lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; + } + + // Update gyro angle + if (gyroInputs.connected) { + // Use the real gyro angle + rawGyroRotation = gyroInputs.odometryYawPositions[i]; + } else { + // Use the angle delta from the kinematics and module deltas + Twist2d twist = kinematics.toTwist2d(moduleDeltas); + rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); + } + + // Apply update + RobotState.getInstance() + .addOdometryObservation( + new OdometryObservation(sampleTimestamps[i], modulePositions, rawGyroRotation)); } + + // Update gyro alert + gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.kCurrentMode != Mode.SIM); } - public void runVelocity(ChassisSpeeds speeds, boolean isOpenLoop) { - // ChassisSpeeds newSpeeds = ChassisSpeeds.discretize(speeds, 0.02); - var states = DriveConstants.swerveKinematics.toSwerveModuleStates(speeds); - SwerveDriveKinematics.desaturateWheelSpeeds(states, DriveConstants.kMaxTeleDriveSpeed); + /** + * Runs the drive at the desired velocity. + * + * @param speeds Speeds in meters/sec + */ + public void runVelocity(ChassisSpeeds speeds) { + // Calculate module setpoints + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); + SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, ModuleConstants.kSpeedAt12Volts); + + // Log unoptimized setpoints and setpoint speeds + Logger.recordOutput("SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("SwerveChassisSpeeds/Setpoints", discreteSpeeds); + + // Send setpoints to modules for (int i = 0; i < 4; i++) { - states[i].optimize(modules[i].getAngle()); - wantedStates[i] = states[i]; - modules[i].runDesiredState(states[i], isOpenLoop); + modules[i].runSetpoint(setpointStates[i]); } + + // Log optimized setpoints (runSetpoint mutates each state) + Logger.recordOutput("SwerveStates/SetpointsOptimized", setpointStates); } - /** Stops all output to the modules' motors. */ - public void stopModules() { - for (SwerveMod mod : modules) { - mod.stop(); + /** Runs the drive in a straight line with the specified drive output. */ + public void runCharacterization(double output) { + for (int i = 0; i < 4; i++) { + modules[i].runCharacterization(output); } } - public double getYawVelocity() { - return this.gyroInputs.data.yawVelocityRadPerSec(); + /** Stops the drive. */ + public void stop() { + runVelocity(new ChassisSpeeds()); } /** - * Retrieves the current position of all swerve modules. - * - * @return An array of {@link SwerveModulePosition} objects, one for each sweve module, ordered - * according to the module array in {@code modules}. + * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will + * return to their normal orientations the next time a nonzero velocity is requested. */ - public SwerveModulePosition[] getModulePositions() { - SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; - - for (int i = 0; i < modules.length; i++) { - modulePositions[i] = modules[i].getPosition(); + public void stopWithX() { + Rotation2d[] headings = new Rotation2d[4]; + for (int i = 0; i < 4; i++) { + headings[i] = getModuleTranslations()[i].getAngle(); } + kinematics.resetHeadings(headings); + stop(); + } - return modulePositions; + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return run(() -> runCharacterization(0.0)) + .withTimeout(1.0) + .andThen(sysId.quasistatic(direction)); } - /** - * Retrieves the current state of all swerve modules. - * - * @return An array of {@link SwerveModuleState} objects, one for each sweve module, ordered - * according to the module array in {@code modules}. - */ + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return run(() -> runCharacterization(0.0)).withTimeout(1.0).andThen(sysId.dynamic(direction)); + } + + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ + @AutoLogOutput(key = "SwerveStates/Measured") public SwerveModuleState[] getModuleStates() { - SwerveModuleState[] moduleStates = new SwerveModuleState[4]; + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getState(); + } + return states; + } - for (int i = 0; i < modules.length; i++) { - moduleStates[i] = modules[i].getState(); + /** Returns the module positions (turn angles and drive positions) for all of the modules. */ + public SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] states = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getPosition(); } + return states; + } - return moduleStates; + /** Returns the measured chassis speeds of the robot. */ + @AutoLogOutput(key = "SwerveChassisSpeeds/Measured") + private ChassisSpeeds getChassisSpeeds() { + return kinematics.toChassisSpeeds(getModuleStates()); } + /** Returns the position of each module in radians. */ + public double[] getWheelRadiusCharacterizationPositions() { + double[] values = new double[4]; + for (int i = 0; i < 4; i++) { + values[i] = modules[i].getWheelRadiusCharacterizationPosition(); + } + return values; + } + + /** Returns the average velocity of the modules in rotations/sec (Phoenix native units). */ + public double getFFCharacterizationVelocity() { + double output = 0.0; + for (int i = 0; i < 4; i++) { + output += modules[i].getFFCharacterizationVelocity() / 4.0; + } + return output; + } + + /** Returns the current gyro rotation. */ public Rotation2d getRawGyroRotation() { - return this.rawGyroRotation; + return rawGyroRotation; + } + + /** Returns the maximum linear speed in meters per sec. */ + public double getMaxLinearSpeedMetersPerSec() { + return ModuleConstants.kSpeedAt12Volts.in(MetersPerSecond); + } + + /** Returns the maximum angular speed in radians per sec. */ + public double getMaxAngularSpeedRadPerSec() { + return getMaxLinearSpeedMetersPerSec() / DriveConstants.kDriveBaseRadius; + } + + /** Returns an array of module translations. */ + public static Translation2d[] getModuleTranslations() { + return new Translation2d[] { + new Translation2d(ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), + new Translation2d(ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY), + new Translation2d(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), + new Translation2d(ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY) + }; } } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 1dac42d..4e9754f 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -1,28 +1,24 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + package frc.robot.subsystems.drive; import edu.wpi.first.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; -/** The {@code GyroIO} interface defines methods and attributes for the gyro. */ public interface GyroIO { - default void updateInputs(GyroIOInputs inputs) {} - ; - @AutoLog - /** Gyro values */ public static class GyroIOInputs { - public GyroIOData data = new GyroIOData(false, Rotation2d.kZero, 0); + public boolean connected = false; + public Rotation2d yawPosition = Rotation2d.kZero; + public double yawVelocityRadPerSec = 0.0; + public double[] odometryYawTimestamps = new double[] {}; + public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; } - public record GyroIOData( - boolean connected, Rotation2d yawPosition, double yawVelocityRadPerSec) {} - ; - - /** Resets the gyro. */ - default void resetGyro() {} - ; - - /** Zeroes the yaw. */ - default void zeroYaw() {} - ; + public default void updateInputs(GyroIOInputs inputs) {} } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java new file mode 100644 index 0000000..6eba69f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -0,0 +1,44 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.drive; + +import com.studica.frc.AHRS; +import com.studica.frc.AHRS.NavXComType; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import frc.robot.Constants.DriveConstants; +import java.util.Queue; + +/** IO implementation for NavX. */ +public class GyroIONavX implements GyroIO { + private final AHRS navX = + new AHRS(NavXComType.kMXP_SPI, (byte) DriveConstants.kOdometryFrequency); + private final Queue yawPositionQueue; + private final Queue yawTimestampQueue; + + public GyroIONavX() { + yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(navX::getYaw); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = navX.isConnected(); + inputs.yawPosition = Rotation2d.fromDegrees(-navX.getYaw()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(-navX.getRawGyroZ()); + + inputs.odometryYawTimestamps = + yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryYawPositions = + yawPositionQueue.stream() + .map((Double value) -> Rotation2d.fromDegrees(-value)) + .toArray(Rotation2d[]::new); + yawTimestampQueue.clear(); + yawPositionQueue.clear(); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java new file mode 100644 index 0000000..6dc7a08 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -0,0 +1,62 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.drive; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.ModuleConstants; +import java.util.Queue; + +/** IO implementation for Pigeon 2. */ +public class GyroIOPigeon2 implements GyroIO { + private final Pigeon2 pigeon = + new Pigeon2(ModuleConstants.DrivetrainConstants.Pigeon2Id, ModuleConstants.kCANBus); + private final StatusSignal yaw = pigeon.getYaw(); + private final Queue yawPositionQueue; + private final Queue yawTimestampQueue; + private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + + public GyroIOPigeon2() { + if (ModuleConstants.DrivetrainConstants.Pigeon2Configs != null) { + pigeon.getConfigurator().apply(ModuleConstants.DrivetrainConstants.Pigeon2Configs); + } else { + pigeon.getConfigurator().apply(new Pigeon2Configuration()); + } + + pigeon.getConfigurator().setYaw(0.0); + yaw.setUpdateFrequency(DriveConstants.kOdometryFrequency); + yawVelocity.setUpdateFrequency(50.0); + pigeon.optimizeBusUtilization(); + yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(yaw.clone()); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + + inputs.odometryYawTimestamps = + yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryYawPositions = + yawPositionQueue.stream() + .map((Double value) -> Rotation2d.fromDegrees(value)) + .toArray(Rotation2d[]::new); + yawTimestampQueue.clear(); + yawPositionQueue.clear(); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java new file mode 100644 index 0000000..8f9781f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -0,0 +1,141 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.drive; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import org.littletonrobotics.junction.Logger; + +public class Module { + private final ModuleIO io; + private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); + private final int index; + private final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + constants; + + private final Alert driveDisconnectedAlert; + private final Alert turnDisconnectedAlert; + private final Alert turnEncoderDisconnectedAlert; + private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; + + public Module( + ModuleIO io, + int index, + SwerveModuleConstants + constants) { + this.io = io; + this.index = index; + this.constants = constants; + driveDisconnectedAlert = + new Alert( + "Disconnected drive motor on module " + Integer.toString(index) + ".", + AlertType.kError); + turnDisconnectedAlert = + new Alert( + "Disconnected turn motor on module " + Integer.toString(index) + ".", AlertType.kError); + turnEncoderDisconnectedAlert = + new Alert( + "Disconnected turn encoder on module " + Integer.toString(index) + ".", + AlertType.kError); + } + + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); + + // Calculate positions for odometry + int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together + odometryPositions = new SwerveModulePosition[sampleCount]; + for (int i = 0; i < sampleCount; i++) { + double positionMeters = inputs.odometryDrivePositionsRad[i] * constants.WheelRadius; + Rotation2d angle = inputs.odometryTurnPositions[i]; + odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + } + + // Update alerts + driveDisconnectedAlert.set(!inputs.driveConnected); + turnDisconnectedAlert.set(!inputs.turnConnected); + turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected); + } + + /** Runs the module with the specified setpoint state. Mutates the state to optimize it. */ + public void runSetpoint(SwerveModuleState state) { + // Optimize velocity setpoint + state.optimize(getAngle()); + state.cosineScale(inputs.turnPosition); + + // Apply setpoints + io.setDriveVelocity(state.speedMetersPerSecond / constants.WheelRadius); + io.setTurnPosition(state.angle); + } + + /** Runs the module with the specified output while controlling to zero degrees. */ + public void runCharacterization(double output) { + io.setDriveOpenLoop(output); + io.setTurnPosition(Rotation2d.kZero); + } + + /** Disables all outputs to motors. */ + public void stop() { + io.setDriveOpenLoop(0.0); + io.setTurnOpenLoop(0.0); + } + + /** Returns the current turn angle of the module. */ + public Rotation2d getAngle() { + return inputs.turnPosition; + } + + /** Returns the current drive position of the module in meters. */ + public double getPositionMeters() { + return inputs.drivePositionRad * constants.WheelRadius; + } + + /** Returns the current drive velocity of the module in meters per second. */ + public double getVelocityMetersPerSec() { + return inputs.driveVelocityRadPerSec * constants.WheelRadius; + } + + /** Returns the module position (turn angle and drive position). */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(getPositionMeters(), getAngle()); + } + + /** Returns the module state (turn angle and drive velocity). */ + public SwerveModuleState getState() { + return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); + } + + /** Returns the module positions received this cycle. */ + public SwerveModulePosition[] getOdometryPositions() { + return odometryPositions; + } + + /** Returns the timestamps of the samples received this cycle. */ + public double[] getOdometryTimestamps() { + return inputs.odometryTimestamps; + } + + /** Returns the module position in radians. */ + public double getWheelRadiusCharacterizationPosition() { + return inputs.drivePositionRad; + } + + /** Returns the module velocity in rotations/sec (Phoenix native units). */ + public double getFFCharacterizationVelocity() { + return Units.radiansToRotations(inputs.driveVelocityRadPerSec); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index ffc9f41..3ad8139 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -1,65 +1,49 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + package frc.robot.subsystems.drive; import edu.wpi.first.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; -/** - * The {@code ModuleIO} class contains default methods for the - * - * @author Maxwell Morgan - */ public interface ModuleIO { - default void updateInputs(ModuleIOInputs inputs) {} - @AutoLog - /** Module values */ - public class ModuleIOInputs { - public ModuleIOData data = - new ModuleIOData(false, 0, 0, 0, false, Rotation2d.kZero, 0, 0, 0, 0); + public static class ModuleIOInputs { + public boolean driveConnected = false; + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double driveCurrentAmps = 0.0; + + public boolean turnConnected = false; + public boolean turnEncoderConnected = false; + public Rotation2d turnAbsolutePosition = Rotation2d.kZero; + public Rotation2d turnPosition = Rotation2d.kZero; + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double turnCurrentAmps = 0.0; + + public double[] odometryTimestamps = new double[] {}; + public double[] odometryDrivePositionsRad = new double[] {}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; } - public record ModuleIOData( - boolean driveConnected, - double drivePositionRad, - double driveVelocityRadPerSec, - double driveAppliedVolts, - boolean turnConnected, - Rotation2d turnPosition, - double turnVelocityRadPerSec, - double turnAppliedVolts, - double driveCurrentAmps, - double turnCurrentAmps) {} - - /** - * Sets the drive motor output. - * - * @param percentOutput the percent of the drive motor's maximum output to request (between -1 and - * 1) - */ - default void runDriveDutyCycle(double percentOutput) {} + /** Updates the set of loggable inputs. */ + public default void updateInputs(ModuleIOInputs inputs) {} - /** - * Sets the turn motor output. - * - * @param percentOutput the percent of the turn motor's maximum output to request (between -1 and - * 1) - */ - default void runTurnDutyCycle(double percentOutput) {} + /** Run the drive motor at the specified open loop value. */ + public default void setDriveOpenLoop(double output) {} - /** - * Sets the drive motor velocity. - * - * @param velocityRadPerSec the velocity used to set the drive motor controller target - */ - default void runDriveVelocity(double velocityRadPerSec) {} + /** Run the turn motor at the specified open loop value. */ + public default void setTurnOpenLoop(double output) {} - /** - * Sets the turn motor to the specified angle. - * - * @param angle the {@link Rotation2d} used to set the angle motor controller target - */ - default void runTurnAngle(Rotation2d angle) {} + /** Run the drive motor at the specified velocity. */ + public default void setDriveVelocity(double velocityRadPerSec) {} - /** Resets the turning encoder to match absolute CANcoder. */ - default void resetToAbsolute() {} + /** Run the turn motor to the specified rotation. */ + public default void setTurnPosition(Rotation2d rotation) {} } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index c74387e..ac73d2b 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -1,100 +1,138 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + package frc.robot.subsystems.drive; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import frc.robot.Constants; -import frc.robot.Constants.DriveConstants; +/** + * Physics sim implementation of module IO. The sim models are configured using a set of module + * constants from Phoenix. Simulation is always based on voltage control. + */ public class ModuleIOSim implements ModuleIO { - private DCMotor driveMotorModel = DCMotor.getKrakenX60(1); - private DCMotor turnMotorModel = DCMotor.getKrakenX44(1); - - private DCMotorSim driveMotorSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - driveMotorModel, 0.025, DriveConstants.kDriveGearRatio), - driveMotorModel); - private DCMotorSim turnMotorSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(turnMotorModel, 0.004, DriveConstants.kAngleGearRatio), - turnMotorModel); + // TunerConstants doesn't support separate sim constants, so they are declared + // locally + private static final double DRIVE_KP = 0.05; + private static final double DRIVE_KD = 0.0; + private static final double DRIVE_KS = 0.0; + private static final double DRIVE_KV_ROT = + 0.91035; // Same units as TunerConstants: (volt * secs) / rotation + private static final double DRIVE_KV = 1.0 / Units.rotationsToRadians(1.0 / DRIVE_KV_ROT); + private static final double TURN_KP = 8.0; + private static final double TURN_KD = 0.0; + private static final DCMotor DRIVE_GEARBOX = DCMotor.getKrakenX60Foc(1); + private static final DCMotor TURN_GEARBOX = DCMotor.getKrakenX60Foc(1); + + private final DCMotorSim driveSim; + private final DCMotorSim turnSim; private boolean driveClosedLoop = false; private boolean turnClosedLoop = false; - private PIDController driveController = new PIDController(0.1, 0, 0.001); - private PIDController turnController = new PIDController(15, 0, 0); - private double driveFFVolts = 0; - private SimpleMotorFeedforward driveFFModel = - new SimpleMotorFeedforward( - DriveConstants.kDriveKS, DriveConstants.kDriveKV, DriveConstants.kDriveKA); + private PIDController driveController = new PIDController(DRIVE_KP, 0, DRIVE_KD); + private PIDController turnController = new PIDController(TURN_KP, 0, TURN_KD); + private double driveFFVolts = 0.0; private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; - public ModuleIOSim() { - // driveFFModel = new SimpleMotorFeedforward(0.0, 0.05); + public ModuleIOSim( + SwerveModuleConstants + constants) { + // Create drive and turn sim models + driveSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DRIVE_GEARBOX, constants.DriveInertia, constants.DriveMotorGearRatio), + DRIVE_GEARBOX); + turnSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + TURN_GEARBOX, constants.SteerInertia, constants.SteerMotorGearRatio), + TURN_GEARBOX); + + // Enable wrapping for turn PID turnController.enableContinuousInput(-Math.PI, Math.PI); } @Override public void updateInputs(ModuleIOInputs inputs) { + // Run closed-loop control if (driveClosedLoop) { driveAppliedVolts = - driveFFVolts + driveController.calculate(driveMotorSim.getAngularVelocityRadPerSec()); + driveFFVolts + driveController.calculate(driveSim.getAngularVelocityRadPerSec()); } else { driveController.reset(); } if (turnClosedLoop) { - turnAppliedVolts = turnController.calculate(turnMotorSim.getAngularPositionRad()); + turnAppliedVolts = turnController.calculate(turnSim.getAngularPositionRad()); } else { turnController.reset(); } - driveMotorSim.setInputVoltage(MathUtil.clamp(driveAppliedVolts, -12.0, 12.0)); - turnMotorSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0)); - driveMotorSim.update(Constants.kLoopPeriodSeconds); - turnMotorSim.update(Constants.kLoopPeriodSeconds); - - inputs.data = - new ModuleIOData( - true, - driveMotorSim.getAngularPositionRad(), - driveMotorSim.getAngularVelocityRadPerSec(), - driveAppliedVolts, - true, - Rotation2d.fromRadians(MathUtil.angleModulus(turnMotorSim.getAngularPositionRad())), - turnMotorSim.getAngularVelocityRadPerSec(), - turnAppliedVolts, - driveMotorSim.getCurrentDrawAmps(), - turnMotorSim.getCurrentDrawAmps()); + // Update simulation state + driveSim.setInputVoltage(MathUtil.clamp(driveAppliedVolts, -12.0, 12.0)); + turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0)); + driveSim.update(0.02); + turnSim.update(0.02); + + // Update drive inputs + inputs.driveConnected = true; + inputs.drivePositionRad = driveSim.getAngularPositionRad(); + inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); + inputs.driveAppliedVolts = driveAppliedVolts; + inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); + + // Update turn inputs + inputs.turnConnected = true; + inputs.turnEncoderConnected = true; + inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()); + inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); + inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); + inputs.turnAppliedVolts = turnAppliedVolts; + inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); + + // Update odometry inputs (50Hz because high-frequency odometry in sim doesn't + // matter) + inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; + inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; + inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; } @Override - public void runDriveDutyCycle(double percentOutput) { + public void setDriveOpenLoop(double output) { driveClosedLoop = false; - driveAppliedVolts = percentOutput * 12; + driveAppliedVolts = output; } @Override - public void runTurnDutyCycle(double percentOutput) { + public void setTurnOpenLoop(double output) { turnClosedLoop = false; - turnAppliedVolts = percentOutput * 12; + turnAppliedVolts = output; } @Override - public void runDriveVelocity(double velocityRadPerSec) { + public void setDriveVelocity(double velocityRadPerSec) { driveClosedLoop = true; - driveFFVolts = 0.75 * driveFFModel.calculate(velocityRadPerSec); + driveFFVolts = DRIVE_KS * Math.signum(velocityRadPerSec) + DRIVE_KV * velocityRadPerSec; driveController.setSetpoint(velocityRadPerSec); } @Override - public void runTurnAngle(Rotation2d angle) { + public void setTurnPosition(Rotation2d rotation) { turnClosedLoop = true; - turnController.setSetpoint(angle.getRadians()); + turnController.setSetpoint(rotation.getRadians()); } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java new file mode 100644 index 0000000..5406907 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -0,0 +1,265 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.drive; + +import static frc.robot.util.PhoenixUtil.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.ParentDevice; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.ModuleConstants; +import java.util.Queue; + +/** + * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and + * CANcoder. Configured using a set of module constants from Phoenix. + * + *

Device configuration and other behaviors not exposed by TunerConstants can be customized here. + */ +public class ModuleIOTalonFX implements ModuleIO { + private final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + constants; + + // Hardware objects + private final TalonFX driveTalon; + private final TalonFX turnTalon; + private final CANcoder cancoder; + + // Voltage control requests + private final VoltageOut voltageRequest = new VoltageOut(0); + private final PositionVoltage positionVoltageRequest = new PositionVoltage(0.0); + private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); + + // Torque-current control requests + private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0); + private final PositionTorqueCurrentFOC positionTorqueCurrentRequest = + new PositionTorqueCurrentFOC(0.0); + private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = + new VelocityTorqueCurrentFOC(0.0); + + // Timestamp inputs from Phoenix thread + private final Queue timestampQueue; + + // Inputs from drive motor + private final StatusSignal drivePosition; + private final Queue drivePositionQueue; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + // Inputs from turn motor + private final StatusSignal turnAbsolutePosition; + private final StatusSignal turnPosition; + private final Queue turnPositionQueue; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + + // Connection debouncers + private final Debouncer driveConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + private final Debouncer turnConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + private final Debouncer turnEncoderConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + public ModuleIOTalonFX( + SwerveModuleConstants + constants) { + this.constants = constants; + driveTalon = new TalonFX(constants.DriveMotorId, ModuleConstants.kCANBus); + turnTalon = new TalonFX(constants.SteerMotorId, ModuleConstants.kCANBus); + cancoder = new CANcoder(constants.EncoderId, ModuleConstants.kCANBus); + + // Configure drive motor + var driveConfig = constants.DriveMotorInitialConfigs; + driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + driveConfig.Slot0 = constants.DriveMotorGains; + driveConfig.Feedback.SensorToMechanismRatio = constants.DriveMotorGearRatio; + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = constants.SlipCurrent; + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -constants.SlipCurrent; + driveConfig.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent; + driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveConfig.MotorOutput.Inverted = + constants.DriveMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); + tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); + + // Configure turn motor + var turnConfig = new TalonFXConfiguration(); + turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + turnConfig.Slot0 = constants.SteerMotorGains; + turnConfig.Feedback.FeedbackRemoteSensorID = constants.EncoderId; + turnConfig.Feedback.FeedbackSensorSource = + switch (constants.FeedbackSource) { + case RemoteCANcoder -> FeedbackSensorSourceValue.RemoteCANcoder; + case FusedCANcoder -> FeedbackSensorSourceValue.FusedCANcoder; + case SyncCANcoder -> FeedbackSensorSourceValue.SyncCANcoder; + default -> throw new RuntimeException( + "You have selected a turn feedback source that is not supported by the default implementation of ModuleIOTalonFX. Please check the AdvantageKit documentation for more information on alternative configurations: https://docs.advantagekit.org/getting-started/template-projects/talonfx-swerve-template#custom-module-implementations"); + }; + turnConfig.Feedback.RotorToSensorRatio = constants.SteerMotorGearRatio; + turnConfig.MotionMagic.MotionMagicCruiseVelocity = 100.0 / constants.SteerMotorGearRatio; + turnConfig.MotionMagic.MotionMagicAcceleration = + turnConfig.MotionMagic.MotionMagicCruiseVelocity / 0.100; + turnConfig.MotionMagic.MotionMagicExpo_kV = 0.12 * constants.SteerMotorGearRatio; + turnConfig.MotionMagic.MotionMagicExpo_kA = 0.1; + turnConfig.ClosedLoopGeneral.ContinuousWrap = true; + turnConfig.MotorOutput.Inverted = + constants.SteerMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25)); + + // Configure CANCoder + CANcoderConfiguration cancoderConfig = constants.EncoderInitialConfigs; + cancoderConfig.MagnetSensor.MagnetOffset = constants.EncoderOffset; + cancoderConfig.MagnetSensor.SensorDirection = + constants.EncoderInverted + ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive; + cancoder.getConfigurator().apply(cancoderConfig); + + // Create timestamp queue + timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + + // Create drive status signals + drivePosition = driveTalon.getPosition(); + drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(drivePosition.clone()); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getStatorCurrent(); + + // Create turn status signals + turnAbsolutePosition = cancoder.getAbsolutePosition(); + turnPosition = turnTalon.getPosition(); + turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(turnPosition.clone()); + turnVelocity = turnTalon.getVelocity(); + turnAppliedVolts = turnTalon.getMotorVoltage(); + turnCurrent = turnTalon.getStatorCurrent(); + + // Configure periodic frames + BaseStatusSignal.setUpdateFrequencyForAll( + DriveConstants.kOdometryFrequency, drivePosition, turnPosition); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); + ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Refresh all signals + var driveStatus = + BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); + var turnStatus = + BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent); + var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); + + // Update drive inputs + inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK()); + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); + + // Update turn inputs + inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK()); + inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK()); + inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); + inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); + + // Update odometry inputs + inputs.odometryTimestamps = + timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryDrivePositionsRad = + drivePositionQueue.stream() + .mapToDouble((Double value) -> Units.rotationsToRadians(value)) + .toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream() + .map((Double value) -> Rotation2d.fromRotations(value)) + .toArray(Rotation2d[]::new); + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveOpenLoop(double output) { + driveTalon.setControl( + switch (constants.DriveMotorClosedLoopOutput) { + case Voltage -> voltageRequest.withOutput(output); + case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); + }); + } + + @Override + public void setTurnOpenLoop(double output) { + turnTalon.setControl( + switch (constants.SteerMotorClosedLoopOutput) { + case Voltage -> voltageRequest.withOutput(output); + case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); + }); + } + + @Override + public void setDriveVelocity(double velocityRadPerSec) { + double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec); + driveTalon.setControl( + switch (constants.DriveMotorClosedLoopOutput) { + case Voltage -> velocityVoltageRequest.withVelocity(velocityRotPerSec); + case TorqueCurrentFOC -> velocityTorqueCurrentRequest.withVelocity(velocityRotPerSec); + }); + } + + @Override + public void setTurnPosition(Rotation2d rotation) { + turnTalon.setControl( + switch (constants.SteerMotorClosedLoopOutput) { + case Voltage -> positionVoltageRequest.withPosition(rotation.getRotations()); + case TorqueCurrentFOC -> positionTorqueCurrentRequest.withPosition( + rotation.getRotations()); + }); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java new file mode 100644 index 0000000..2fd066e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java @@ -0,0 +1,252 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.drive; + +import static frc.robot.util.PhoenixUtil.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANdiConfiguration; +import com.ctre.phoenix6.configs.TalonFXSConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANdi; +import com.ctre.phoenix6.hardware.ParentDevice; +import com.ctre.phoenix6.hardware.TalonFXS; +import com.ctre.phoenix6.signals.BrushedMotorWiringValue; +import com.ctre.phoenix6.signals.ExternalFeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorArrangementValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.ModuleConstants; +import java.util.Queue; + +/** + * Module IO implementation for Talon FXS drive motor controller, Talon FXS turn motor controller, + * and CANdi (PWM 1). Configured using a set of module constants from Phoenix. + * + *

Device configuration and other behaviors not exposed by TunerConstants can be customized here. + */ +public class ModuleIOTalonFXS implements ModuleIO { + // Hardware objects + private final TalonFXS driveTalon; + private final TalonFXS turnTalon; + private final CANdi candi; + + // Voltage control requests + private final VoltageOut voltageRequest = new VoltageOut(0); + private final PositionVoltage positionVoltageRequest = new PositionVoltage(0.0); + private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); + + // Timestamp inputs from Phoenix thread + private final Queue timestampQueue; + + // Inputs from drive motor + private final StatusSignal drivePosition; + private final Queue drivePositionQueue; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + // Inputs from turn motor + private final StatusSignal turnAbsolutePosition; + private final StatusSignal turnPosition; + private final Queue turnPositionQueue; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + + // Connection debouncers + private final Debouncer driveConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + private final Debouncer turnConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + private final Debouncer turnEncoderConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + public ModuleIOTalonFXS( + SwerveModuleConstants + constants) { + driveTalon = new TalonFXS(constants.DriveMotorId, ModuleConstants.kCANBus); + turnTalon = new TalonFXS(constants.SteerMotorId, ModuleConstants.kCANBus); + candi = new CANdi(constants.EncoderId, ModuleConstants.kCANBus); + + // Configure drive motor + var driveConfig = constants.DriveMotorInitialConfigs; + driveConfig.Commutation.MotorArrangement = + switch (constants.DriveMotorType) { + case TalonFXS_NEO_JST -> MotorArrangementValue.NEO_JST; + case TalonFXS_VORTEX_JST -> MotorArrangementValue.VORTEX_JST; + default -> MotorArrangementValue.Disabled; + }; + driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + driveConfig.Slot0 = constants.DriveMotorGains; + driveConfig.ExternalFeedback.SensorToMechanismRatio = constants.DriveMotorGearRatio; + driveConfig.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent; + driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveConfig.MotorOutput.Inverted = + constants.DriveMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); + tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); + + // Configure turn motor + var turnConfig = new TalonFXSConfiguration(); + turnConfig.Commutation.MotorArrangement = + switch (constants.SteerMotorType) { + case TalonFXS_Minion_JST -> MotorArrangementValue.Minion_JST; + case TalonFXS_NEO_JST -> MotorArrangementValue.NEO_JST; + case TalonFXS_VORTEX_JST -> MotorArrangementValue.VORTEX_JST; + case TalonFXS_NEO550_JST -> MotorArrangementValue.NEO550_JST; + case TalonFXS_Brushed_AB, + TalonFXS_Brushed_AC, + TalonFXS_Brushed_BC -> MotorArrangementValue.Brushed_DC; + default -> MotorArrangementValue.Disabled; + }; + turnConfig.Commutation.BrushedMotorWiring = + switch (constants.SteerMotorType) { + case TalonFXS_Brushed_AC -> BrushedMotorWiringValue.Leads_A_and_C; + case TalonFXS_Brushed_BC -> BrushedMotorWiringValue.Leads_B_and_C; + default -> BrushedMotorWiringValue.Leads_A_and_B; + }; + turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + turnConfig.Slot0 = constants.SteerMotorGains; + turnConfig.ExternalFeedback.FeedbackRemoteSensorID = constants.EncoderId; + turnConfig.ExternalFeedback.ExternalFeedbackSensorSource = + switch (constants.FeedbackSource) { + case RemoteCANdiPWM1 -> ExternalFeedbackSensorSourceValue.RemoteCANdiPWM1; + case FusedCANdiPWM1 -> ExternalFeedbackSensorSourceValue.FusedCANdiPWM1; + case SyncCANdiPWM1 -> ExternalFeedbackSensorSourceValue.SyncCANdiPWM1; + default -> throw new RuntimeException( + "You have selected a turn feedback source that is not supported by the default implementation of ModuleIOTalonFXS (CANdi PWM 1). Please check the AdvantageKit documentation for more information on alternative configurations: https://docs.advantagekit.org/getting-started/template-projects/talonfx-swerve-template#custom-module-implementations"); + }; + turnConfig.ExternalFeedback.RotorToSensorRatio = constants.SteerMotorGearRatio; + turnConfig.MotionMagic.MotionMagicCruiseVelocity = 100.0 / constants.SteerMotorGearRatio; + turnConfig.MotionMagic.MotionMagicAcceleration = + turnConfig.MotionMagic.MotionMagicCruiseVelocity / 0.100; + turnConfig.MotionMagic.MotionMagicExpo_kV = 0.12 * constants.SteerMotorGearRatio; + turnConfig.MotionMagic.MotionMagicExpo_kA = 0.1; + turnConfig.ClosedLoopGeneral.ContinuousWrap = true; + turnConfig.MotorOutput.Inverted = + constants.SteerMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25)); + + // Configure CANdi + CANdiConfiguration candiConfig = constants.EncoderInitialConfigs; + candiConfig.PWM1.AbsoluteSensorOffset = constants.EncoderOffset; + candiConfig.PWM1.SensorDirection = constants.EncoderInverted; + candi.getConfigurator().apply(candiConfig); + + // Create timestamp queue + timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + + // Create drive status signals + drivePosition = driveTalon.getPosition(); + drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(drivePosition.clone()); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getStatorCurrent(); + + // Create turn status signals + turnAbsolutePosition = candi.getPWM1Position(); + turnPosition = turnTalon.getPosition(); + turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(turnPosition.clone()); + turnVelocity = turnTalon.getVelocity(); + turnAppliedVolts = turnTalon.getMotorVoltage(); + turnCurrent = turnTalon.getStatorCurrent(); + + // Configure periodic frames + BaseStatusSignal.setUpdateFrequencyForAll( + DriveConstants.kOdometryFrequency, drivePosition, turnPosition); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); + ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Refresh all signals + var driveStatus = + BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); + var turnStatus = + BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent); + var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); + + // Update drive inputs + inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK()); + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); + + // Update turn inputs + inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK()); + inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK()); + inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); + inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); + + // Update odometry inputs + inputs.odometryTimestamps = + timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryDrivePositionsRad = + drivePositionQueue.stream() + .mapToDouble((Double value) -> Units.rotationsToRadians(value)) + .toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream() + .map((Double value) -> Rotation2d.fromRotations(value)) + .toArray(Rotation2d[]::new); + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveOpenLoop(double output) { + driveTalon.setControl(voltageRequest.withOutput(output)); + } + + @Override + public void setTurnOpenLoop(double output) { + turnTalon.setControl(voltageRequest.withOutput(output)); + } + + @Override + public void setDriveVelocity(double velocityRadPerSec) { + double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec); + driveTalon.setControl(velocityVoltageRequest.withVelocity(velocityRotPerSec)); + } + + @Override + public void setTurnPosition(Rotation2d rotation) { + turnTalon.setControl(positionVoltageRequest.withPosition(rotation.getRotations())); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java new file mode 100644 index 0000000..5b87a66 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -0,0 +1,159 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.drive; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.ModuleConstants; +import java.util.ArrayList; +import java.util.List; +import java.util.Queue; +import java.util.concurrent.ArrayBlockingQueue; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import java.util.function.DoubleSupplier; + +/** + * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. + * + *

This version is intended for Phoenix 6 devices on both the RIO and CANivore buses. When using + * a CANivore, the thread uses the "waitForAll" blocking method to enable more consistent sampling. + * This also allows Phoenix Pro users to benefit from lower latency between devices using CANivore + * time synchronization. + */ +public class PhoenixOdometryThread extends Thread { + private final Lock signalsLock = + new ReentrantLock(); // Prevents conflicts when registering signals + private BaseStatusSignal[] phoenixSignals = new BaseStatusSignal[0]; + private final List genericSignals = new ArrayList<>(); + private final List> phoenixQueues = new ArrayList<>(); + private final List> genericQueues = new ArrayList<>(); + private final List> timestampQueues = new ArrayList<>(); + + private static boolean isCANFD = ModuleConstants.kCANBus.isNetworkFD(); + private static PhoenixOdometryThread instance = null; + + public static PhoenixOdometryThread getInstance() { + if (instance == null) { + instance = new PhoenixOdometryThread(); + } + return instance; + } + + private PhoenixOdometryThread() { + setName("PhoenixOdometryThread"); + setDaemon(true); + } + + @Override + public void start() { + if (timestampQueues.size() > 0) { + super.start(); + } + } + + /** Registers a Phoenix signal to be read from the thread. */ + public Queue registerSignal(StatusSignal signal) { + Queue queue = new ArrayBlockingQueue<>(20); + signalsLock.lock(); + Drive.odometryLock.lock(); + try { + BaseStatusSignal[] newSignals = new BaseStatusSignal[phoenixSignals.length + 1]; + System.arraycopy(phoenixSignals, 0, newSignals, 0, phoenixSignals.length); + newSignals[phoenixSignals.length] = signal; + phoenixSignals = newSignals; + phoenixQueues.add(queue); + } finally { + signalsLock.unlock(); + Drive.odometryLock.unlock(); + } + return queue; + } + + /** Registers a generic signal to be read from the thread. */ + public Queue registerSignal(DoubleSupplier signal) { + Queue queue = new ArrayBlockingQueue<>(20); + signalsLock.lock(); + Drive.odometryLock.lock(); + try { + genericSignals.add(signal); + genericQueues.add(queue); + } finally { + signalsLock.unlock(); + Drive.odometryLock.unlock(); + } + return queue; + } + + /** Returns a new queue that returns timestamp values for each sample. */ + public Queue makeTimestampQueue() { + Queue queue = new ArrayBlockingQueue<>(20); + Drive.odometryLock.lock(); + try { + timestampQueues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } + + @Override + public void run() { + while (true) { + // Wait for updates from all signals + signalsLock.lock(); + try { + if (isCANFD && phoenixSignals.length > 0) { + BaseStatusSignal.waitForAll(2.0 / DriveConstants.kOdometryFrequency, phoenixSignals); + } else { + // "waitForAll" does not support blocking on multiple signals with a bus + // that is not CAN FD, regardless of Pro licensing. No reasoning for this + // behavior is provided by the documentation. + Thread.sleep((long) (1000.0 / DriveConstants.kOdometryFrequency)); + if (phoenixSignals.length > 0) BaseStatusSignal.refreshAll(phoenixSignals); + } + } catch (InterruptedException e) { + e.printStackTrace(); + } finally { + signalsLock.unlock(); + } + + // Save new data to queues + Drive.odometryLock.lock(); + try { + // Sample timestamp is current FPGA time minus average CAN latency + // Default timestamps from Phoenix are NOT compatible with + // FPGA timestamps, this solution is imperfect but close + double timestamp = RobotController.getFPGATime() / 1e6; + double totalLatency = 0.0; + for (BaseStatusSignal signal : phoenixSignals) { + totalLatency += signal.getTimestamp().getLatency(); + } + if (phoenixSignals.length > 0) { + timestamp -= totalLatency / phoenixSignals.length; + } + + // Add new samples to queues + for (int i = 0; i < phoenixSignals.length; i++) { + phoenixQueues.get(i).offer(phoenixSignals[i].getValueAsDouble()); + } + for (int i = 0; i < genericSignals.size(); i++) { + genericQueues.get(i).offer(genericSignals.get(i).getAsDouble()); + } + for (int i = 0; i < timestampQueues.size(); i++) { + timestampQueues.get(i).offer(timestamp); + } + } finally { + Drive.odometryLock.unlock(); + } + } + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveMod.java b/src/main/java/frc/robot/subsystems/drive/SwerveMod.java deleted file mode 100644 index 69bdf7b..0000000 --- a/src/main/java/frc/robot/subsystems/drive/SwerveMod.java +++ /dev/null @@ -1,108 +0,0 @@ -package frc.robot.subsystems.drive; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import frc.robot.Constants.DriveConstants; -import org.littletonrobotics.junction.Logger; - -/** - * The {@code SwerveMod} class contains/controls the io, inputs, and name of one swerve module. - * - * @author Maxwell Morgan - */ -public class SwerveMod { - private final ModuleIO io; - - private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); - private final ModuleName name; - - public SwerveMod(ModuleIO io, ModuleName name) { - this.io = io; - this.name = name; - } - - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Drive/Module/" + name.toString(), inputs); - } - - /** - * Runs the module to the desired state. - * - * @param desiredState Desired wheel speed and angle - * @param isOpenLoop If true, uses percent output; if false, uses velocity closed-loop - */ - public void runDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) { - // Cosine compensation reduces speed if wheel isn't pointing correctly - double angleDiff = desiredState.angle.minus(getAngle()).getRadians(); - double compensatedSpeed = desiredState.speedMetersPerSecond * Math.cos(angleDiff); - double speedRadPerSec = compensatedSpeed / DriveConstants.kWheelRadius; - - if (isOpenLoop) { - // In meters per second - double percentOutput = compensatedSpeed / DriveConstants.kPhysicalMaxSpeed; - io.runDriveDutyCycle(percentOutput); - } else { - io.runDriveVelocity(speedRadPerSec); - } - - // Turn control - if (isTurnWithinDeadband(desiredState.angle)) { - io.runTurnDutyCycle(0); - } else { - io.runTurnAngle(desiredState.angle); - } - } - - /** Stops all output to the module's motors. */ - public void stop() { - io.runDriveDutyCycle(0); - io.runTurnDutyCycle(0); - } - - public void resetToAbsolute() { - io.resetToAbsolute(); - } - - public SwerveModuleState getState() { - return new SwerveModuleState(getVelocityMetersPerSec(), inputs.data.turnPosition()); - } - - public SwerveModulePosition getPosition() { - return new SwerveModulePosition(getPositionMeters(), inputs.data.turnPosition()); - } - - public Rotation2d getAngle() { - return inputs.data.turnPosition(); - } - - public double getPositionMeters() { - return inputs.data.drivePositionRad() * DriveConstants.kWheelRadius; - } - - public double getVelocityMetersPerSec() { - return inputs.data.driveVelocityRadPerSec() * DriveConstants.kWheelRadius; - } - - private boolean isTurnWithinDeadband(Rotation2d target) { - return Math.abs(target.minus(getAngle()).getDegrees()) < 3; - } - - public enum ModuleName { - FRONT_LEFT(0), - FRONT_RIGHT(1), - BACK_LEFT(2), - BACK_RIGHT(3); - - private final int index; - - ModuleName(int index) { - this.index = index; - } - - public int getIndex() { - return this.index; - } - } -} diff --git a/src/main/java/frc/robot/util/AllianceFlipUtil.java b/src/main/java/frc/robot/util/AllianceFlipUtil.java new file mode 100644 index 0000000..9b50d6e --- /dev/null +++ b/src/main/java/frc/robot/util/AllianceFlipUtil.java @@ -0,0 +1,55 @@ +// Copyright (c) 2025-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.util; + +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.Constants; + +public class AllianceFlipUtil { + public static double applyX(double x) { + return shouldFlip() ? FieldConstants.fieldLength - x : x; + } + + public static double applyY(double y) { + return shouldFlip() ? FieldConstants.fieldWidth - y : y; + } + + public static Translation2d apply(Translation2d translation) { + return new Translation2d(applyX(translation.getX()), applyY(translation.getY())); + } + + public static Rotation2d apply(Rotation2d rotation) { + return shouldFlip() ? rotation.rotateBy(Rotation2d.kPi) : rotation; + } + + public static Pose2d apply(Pose2d pose) { + return shouldFlip() + ? new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())) + : pose; + } + + public static Translation3d apply(Translation3d translation) { + return new Translation3d( + applyX(translation.getX()), applyY(translation.getY()), translation.getZ()); + } + + public static Rotation3d apply(Rotation3d rotation) { + return shouldFlip() ? rotation.rotateBy(new Rotation3d(0.0, 0.0, Math.PI)) : rotation; + } + + public static Pose3d apply(Pose3d pose) { + return new Pose3d(apply(pose.getTranslation()), apply(pose.getRotation())); + } + + public static boolean shouldFlip() { + return !Constants.kDisableHAL + && DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + } +} diff --git a/src/main/java/frc/robot/util/FieldConstants.java b/src/main/java/frc/robot/util/FieldConstants.java new file mode 100644 index 0000000..218d3c9 --- /dev/null +++ b/src/main/java/frc/robot/util/FieldConstants.java @@ -0,0 +1,339 @@ +// Copyright (c) 2025-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.util; + +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.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; + +/** + * Contains information for location of field element and other useful reference points. + * + *

NOTE: All constants are defined relative to the field coordinate system, and from the + * perspective of the blue alliance station + */ +public class FieldConstants { + public static final FieldType fieldType = FieldType.ANDYMARK; + + // AprilTag related constants + public static final int aprilTagCount = AprilTagLayoutType.OFFICIAL.getLayout().getTags().size(); + public static final double aprilTagWidth = Units.inchesToMeters(6.5); + public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL; + + // Field dimensions + public static final double fieldLength = AprilTagLayoutType.OFFICIAL.getLayout().getFieldLength(); + public static final double fieldWidth = AprilTagLayoutType.OFFICIAL.getLayout().getFieldWidth(); + + /** + * Officially defined and relevant vertical lines found on the field (defined by X-axis offset) + */ + public static class LinesVertical { + public static final double center = fieldLength / 2.0; + public static final double starting = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX(); + public static final double allianceZone = starting; + public static final double hubCenter = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + Hub.width / 2.0; + public static final double neutralZoneNear = center - Units.inchesToMeters(120); + public static final double neutralZoneFar = center + Units.inchesToMeters(120); + public static final double oppHubCenter = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(4).get().getX() + Hub.width / 2.0; + public static final double oppAllianceZone = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(10).get().getX(); + } + + /** + * Officially defined and relevant horizontal lines found on the field (defined by Y-axis offset) + * + *

NOTE: The field element start and end are always left to right from the perspective of the + * alliance station + */ + public static class LinesHorizontal { + + public static final double center = fieldWidth / 2.0; + + // Right of hub + public static final double rightBumpStart = Hub.nearRightCorner.getY(); + public static final double rightBumpEnd = rightBumpStart - RightBump.width; + public static final double rightTrenchOpenStart = rightBumpEnd - Units.inchesToMeters(12.0); + public static final double rightTrenchOpenEnd = 0; + + // Left of hub + public static final double leftBumpEnd = Hub.nearLeftCorner.getY(); + public static final double leftBumpStart = leftBumpEnd + LeftBump.width; + public static final double leftTrenchOpenEnd = leftBumpStart + Units.inchesToMeters(12.0); + public static final double leftTrenchOpenStart = fieldWidth; + } + + /** Hub related constants */ + public static class Hub { + + // Dimensions + public static final double width = Units.inchesToMeters(47.0); + public static final double height = + Units.inchesToMeters(72.0); // includes the catcher at the top + public static final double innerWidth = Units.inchesToMeters(41.7); + public static final double innerHeight = Units.inchesToMeters(56.5); + + // Relevant reference points on alliance side + public static final Translation3d topCenterPoint = + new Translation3d( + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + width / 2.0, + fieldWidth / 2.0, + height); + public static final Translation3d innerCenterPoint = + new Translation3d( + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + width / 2.0, + fieldWidth / 2.0, + innerHeight); + + public static final Translation2d nearLeftCorner = + new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d nearRightCorner = + new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); + public static final Translation2d farLeftCorner = + new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d farRightCorner = + new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); + + // Relevant reference points on the opposite side + public static final Translation3d oppTopCenterPoint = + new Translation3d( + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(4).get().getX() + width / 2.0, + fieldWidth / 2.0, + height); + public static final Translation2d oppNearLeftCorner = + new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d oppNearRightCorner = + new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); + public static final Translation2d oppFarLeftCorner = + new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d oppFarRightCorner = + new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); + + // Hub faces + public static final Pose2d nearFace = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().toPose2d(); + public static final Pose2d farFace = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(20).get().toPose2d(); + public static final Pose2d rightFace = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(18).get().toPose2d(); + public static final Pose2d leftFace = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(21).get().toPose2d(); + } + + /** Left Bump related constants */ + public static class LeftBump { + + // Dimensions + public static final double width = Units.inchesToMeters(73.0); + public static final double height = Units.inchesToMeters(6.513); + public static final double depth = Units.inchesToMeters(44.4); + + // Relevant reference points on alliance side + public static final Translation2d nearLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d nearRightCorner = Hub.nearLeftCorner; + public static final Translation2d farLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d farRightCorner = Hub.farLeftCorner; + + // Relevant reference points on opposing side + public static final Translation2d oppNearLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; + public static final Translation2d oppFarLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; + } + + /** Right Bump related constants */ + public static class RightBump { + // Dimensions + public static final double width = Units.inchesToMeters(73.0); + public static final double height = Units.inchesToMeters(6.513); + public static final double depth = Units.inchesToMeters(44.4); + + // Relevant reference points on alliance side + public static final Translation2d nearLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d nearRightCorner = Hub.nearLeftCorner; + public static final Translation2d farLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d farRightCorner = Hub.farLeftCorner; + + // Relevant reference points on opposing side + public static final Translation2d oppNearLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; + public static final Translation2d oppFarLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; + } + + /** Left Trench related constants */ + public static class LeftTrench { + // Dimensions + public static final double width = Units.inchesToMeters(65.65); + public static final double depth = Units.inchesToMeters(47.0); + public static final double height = Units.inchesToMeters(40.25); + public static final double openingWidth = Units.inchesToMeters(50.34); + public static final double openingHeight = Units.inchesToMeters(22.25); + + // Relevant reference points on alliance side + public static final Translation3d openingTopLeft = + new Translation3d(LinesVertical.hubCenter, fieldWidth, openingHeight); + public static final Translation3d openingTopRight = + new Translation3d(LinesVertical.hubCenter, fieldWidth - openingWidth, openingHeight); + + // Relevant reference points on opposing side + public static final Translation3d oppOpeningTopLeft = + new Translation3d(LinesVertical.oppHubCenter, fieldWidth, openingHeight); + public static final Translation3d oppOpeningTopRight = + new Translation3d(LinesVertical.oppHubCenter, fieldWidth - openingWidth, openingHeight); + } + + public static class RightTrench { + + // Dimensions + public static final double width = Units.inchesToMeters(65.65); + public static final double depth = Units.inchesToMeters(47.0); + public static final double height = Units.inchesToMeters(40.25); + public static final double openingWidth = Units.inchesToMeters(50.34); + public static final double openingHeight = Units.inchesToMeters(22.25); + + // Relevant reference points on alliance side + public static final Translation3d openingTopLeft = + new Translation3d(LinesVertical.hubCenter, openingWidth, openingHeight); + public static final Translation3d openingTopRight = + new Translation3d(LinesVertical.hubCenter, 0, openingHeight); + + // Relevant reference points on opposing side + public static final Translation3d oppOpeningTopLeft = + new Translation3d(LinesVertical.oppHubCenter, openingWidth, openingHeight); + public static final Translation3d oppOpeningTopRight = + new Translation3d(LinesVertical.oppHubCenter, 0, openingHeight); + } + + /** Tower related constants */ + public static class Tower { + // Dimensions + public static final double width = Units.inchesToMeters(49.25); + public static final double depth = Units.inchesToMeters(45.0); + public static final double height = Units.inchesToMeters(78.25); + public static final double innerOpeningWidth = Units.inchesToMeters(32.250); + public static final double frontFaceX = Units.inchesToMeters(43.51); + + public static final double uprightHeight = Units.inchesToMeters(72.1); + + // Rung heights from the floor + public static final double lowRungHeight = Units.inchesToMeters(27.0); + public static final double midRungHeight = Units.inchesToMeters(45.0); + public static final double highRungHeight = Units.inchesToMeters(63.0); + + // Relevant reference points on alliance side + public static final Translation2d centerPoint = + new Translation2d( + frontFaceX, AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()); + public static final Translation2d leftUpright = + new Translation2d( + frontFaceX, + (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()) + + innerOpeningWidth / 2 + + Units.inchesToMeters(0.75)); + public static final Translation2d rightUpright = + new Translation2d( + frontFaceX, + (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()) + - innerOpeningWidth / 2 + - Units.inchesToMeters(0.75)); + + // Relevant reference points on opposing side + public static final Translation2d oppCenterPoint = + new Translation2d( + fieldLength - frontFaceX, + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()); + public static final Translation2d oppLeftUpright = + new Translation2d( + fieldLength - frontFaceX, + (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()) + + innerOpeningWidth / 2 + + Units.inchesToMeters(0.75)); + public static final Translation2d oppRightUpright = + new Translation2d( + fieldLength - frontFaceX, + (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()) + - innerOpeningWidth / 2 + - Units.inchesToMeters(0.75)); + } + + public static class Depot { + // Dimensions + public static final double width = Units.inchesToMeters(42.0); + public static final double depth = Units.inchesToMeters(27.0); + public static final double height = Units.inchesToMeters(1.125); + public static final double distanceFromCenterY = Units.inchesToMeters(75.93); + + // Relevant reference points on alliance side + public static final Translation3d depotCenter = + new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY, height); + public static final Translation3d leftCorner = + new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY + (width / 2), height); + public static final Translation3d rightCorner = + new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY - (width / 2), height); + } + + public static class Outpost { + // Dimensions + public static final double width = Units.inchesToMeters(31.8); + public static final double openingDistanceFromFloor = Units.inchesToMeters(28.1); + public static final double height = Units.inchesToMeters(7.0); + + // Relevant reference points on alliance side + public static final Translation2d centerPoint = + new Translation2d(0, AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(29).get().getY()); + } + + public enum FieldType { + ANDYMARK(AprilTagFields.k2026RebuiltAndymark), + WELDED(AprilTagFields.k2026RebuiltWelded); + + private final AprilTagFields aprilTagField; + + FieldType(AprilTagFields aprilTagField) { + this.aprilTagField = aprilTagField; + } + + public AprilTagFields getAprilTagField() { + return aprilTagField; + } + } + + public enum AprilTagLayoutType { + OFFICIAL, + NONE; + + private volatile AprilTagFieldLayout layout; + + AprilTagLayoutType() {} + + public AprilTagFieldLayout getLayout() { + if (layout == null) { + synchronized (this) { + if (layout == null) { + layout = AprilTagFieldLayout.loadField(fieldType.getAprilTagField()); + } + } + } + return layout; + } + } +} diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java new file mode 100644 index 0000000..846dd04 --- /dev/null +++ b/src/main/java/frc/robot/util/LocalADStarAK.java @@ -0,0 +1,160 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.util; + +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPoint; +import com.pathplanner.lib.pathfinding.LocalADStar; +import com.pathplanner.lib.pathfinding.Pathfinder; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Translation2d; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +// NOTE: This file is available at +// https://gist.github.com/mjansen4857/a8024b55eb427184dbd10ae8923bd57d + +public class LocalADStarAK implements Pathfinder { + private final ADStarIO io = new ADStarIO(); + + /** + * Get if a new path has been calculated since the last time a path was retrieved + * + * @return True if a new path is available + */ + @Override + public boolean isNewPathAvailable() { + if (!Logger.hasReplaySource()) { + io.updateIsNewPathAvailable(); + } + + Logger.processInputs("LocalADStarAK", io); + + return io.isNewPathAvailable; + } + + /** + * Get the most recently calculated path + * + * @param constraints The path constraints to use when creating the path + * @param goalEndState The goal end state to use when creating the path + * @return The PathPlannerPath created from the points calculated by the pathfinder + */ + @Override + public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { + if (!Logger.hasReplaySource()) { + io.updateCurrentPathPoints(constraints, goalEndState); + } + + Logger.processInputs("LocalADStarAK", io); + + if (io.currentPathPoints.isEmpty()) { + return null; + } + + return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); + } + + /** + * Set the start position to pathfind from + * + * @param startPosition Start position on the field. If this is within an obstacle it will be + * moved to the nearest non-obstacle node. + */ + @Override + public void setStartPosition(Translation2d startPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setStartPosition(startPosition); + } + } + + /** + * Set the goal position to pathfind to + * + * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved + * to the nearest non-obstacle node. + */ + @Override + public void setGoalPosition(Translation2d goalPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setGoalPosition(goalPosition); + } + } + + /** + * Set the dynamic obstacles that should be avoided while pathfinding. + * + * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents + * opposite corners of a bounding box. + * @param currentRobotPos The current position of the robot. This is needed to change the start + * position of the path to properly avoid obstacles + */ + @Override + public void setDynamicObstacles( + List> obs, Translation2d currentRobotPos) { + if (!Logger.hasReplaySource()) { + io.adStar.setDynamicObstacles(obs, currentRobotPos); + } + } + + private static class ADStarIO implements LoggableInputs { + public LocalADStar adStar = new LocalADStar(); + public boolean isNewPathAvailable = false; + public List currentPathPoints = Collections.emptyList(); + + @Override + public void toLog(LogTable table) { + table.put("IsNewPathAvailable", isNewPathAvailable); + + double[] pointsLogged = new double[currentPathPoints.size() * 2]; + int idx = 0; + for (PathPoint point : currentPathPoints) { + pointsLogged[idx] = point.position.getX(); + pointsLogged[idx + 1] = point.position.getY(); + idx += 2; + } + + table.put("CurrentPathPoints", pointsLogged); + } + + @Override + public void fromLog(LogTable table) { + isNewPathAvailable = table.get("IsNewPathAvailable", false); + + double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); + + List pathPoints = new ArrayList<>(); + for (int i = 0; i < pointsLogged.length; i += 2) { + pathPoints.add( + new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); + } + + currentPathPoints = pathPoints; + } + + public void updateIsNewPathAvailable() { + isNewPathAvailable = adStar.isNewPathAvailable(); + } + + public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { + PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); + + if (currentPath != null) { + currentPathPoints = currentPath.getAllPathPoints(); + } else { + currentPathPoints = Collections.emptyList(); + } + } + } +} diff --git a/src/main/java/frc/robot/util/PhoenixUtil.java b/src/main/java/frc/robot/util/PhoenixUtil.java new file mode 100644 index 0000000..1f37d62 --- /dev/null +++ b/src/main/java/frc/robot/util/PhoenixUtil.java @@ -0,0 +1,21 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.util; + +import com.ctre.phoenix6.StatusCode; +import java.util.function.Supplier; + +public class PhoenixUtil { + /** Attempts to run the command until no error is produced. */ + public static void tryUntilOk(int maxAttempts, Supplier command) { + for (int i = 0; i < maxAttempts; i++) { + var error = command.get(); + if (error.isOK()) break; + } + } +} diff --git a/vendordeps/Studica.json b/vendordeps/Studica.json new file mode 100644 index 0000000..daf1434 --- /dev/null +++ b/vendordeps/Studica.json @@ -0,0 +1,71 @@ +{ + "fileName": "Studica.json", + "name": "Studica", + "version": "2026.0.0", + "frcYear": "2026", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2026/" + ], + "jsonUrl": "https://dev.studica.com/maven/release/2026/json/Studica-2026.0.0.json", + "javaDependencies": [ + { + "groupId": "com.studica.frc", + "artifactId": "Studica-java", + "version": "2026.0.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.studica.frc", + "artifactId": "Studica-driver", + "version": "2026.0.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.studica.frc", + "artifactId": "Studica-cpp", + "version": "2026.0.0", + "libName": "Studica", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.studica.frc", + "artifactId": "Studica-driver", + "version": "2026.0.0", + "libName": "StudicaDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} From 6334e0288e6b09504d7cec6f3341ac744fa3133b Mon Sep 17 00:00:00 2001 From: Maxwell Morgan Date: Fri, 23 Jan 2026 17:38:46 -0500 Subject: [PATCH 08/61] Update vendor deps --- .vscode/settings.json | 2 +- vendordeps/REVLib.json | 18 +++++++++--------- vendordeps/StudicaLib.json | 19 +++++++++++++------ vendordeps/photonlib.json | 12 ++++++------ 4 files changed, 29 insertions(+), 22 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 2290187..e981dcf 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -70,5 +70,5 @@ "[java]": { "editor.defaultFormatter": "redhat.java" }, - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" } diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 082b01d..bb613bf 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2026.0.0", + "version": "2026.0.1", "frcYear": "2026", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2026.0.0" + "version": "2026.0.1" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.0", + "version": "2026.0.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.0", + "version": "2026.0.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -49,7 +49,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.0", + "version": "2026.0.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -66,7 +66,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2026.0.0", + "version": "2026.0.1", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -83,7 +83,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.0", + "version": "2026.0.1", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -100,7 +100,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.0", + "version": "2026.0.1", "libName": "BackendDriver", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -116,7 +116,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.0", + "version": "2026.0.1", "libName": "REVLibWpi", "sharedLibrary": true, "skipInvalidPlatforms": true, diff --git a/vendordeps/StudicaLib.json b/vendordeps/StudicaLib.json index d7ad2e7..4b4a823 100644 --- a/vendordeps/StudicaLib.json +++ b/vendordeps/StudicaLib.json @@ -1,25 +1,32 @@ { "fileName": "StudicaLib.json", "name": "StudicaLib", - "version": "2026.0.0", + "version": "2026.0.1", "frcYear": "2026", "uuid": "963ef341-8abd-4981-a969-c8ae3f592e82", "mavenUrls": [ "https://dev.studica.com/maven/release/2026/" ], - "jsonUrl": "https://dev.studica.com/maven/release/2026/json/StudicaLib-2026.0.0.json", + "jsonUrl": "https://dev.studica.com/maven/release/2026/json/StudicaLib-2026.0.1.json", + "conflictsWith": [ + { + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "errorMessage": "StudicaLib (newer devices) is not compatible with Studica (NavX, NavX2)", + "offlineFileName": "Studica.json" + } + ], "javaDependencies": [ { "groupId": "com.studica.frc", "artifactId": "StudicaLib-java", - "version": "2026.0.0" + "version": "2026.0.1" } ], "jniDependencies": [ { "groupId": "com.studica.frc", "artifactId": "StudicaLib-driver", - "version": "2026.0.0", + "version": "2026.0.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -36,7 +43,7 @@ { "groupId": "com.studica.frc", "artifactId": "StudicaLib-cpp", - "version": "2026.0.0", + "version": "2026.0.1", "libName": "StudicaLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -53,7 +60,7 @@ { "groupId": "com.studica.frc", "artifactId": "StudicaLib-driver", - "version": "2026.0.0", + "version": "2026.0.1", "libName": "StudicaLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 7508481..b0ac8fb 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2026.1.1-rc-3", + "version": "v2026.1.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1-rc-3", + "version": "v2026.1.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2026.1.1-rc-3", + "version": "v2026.1.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1-rc-3", + "version": "v2026.1.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2026.1.1-rc-3" + "version": "v2026.1.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2026.1.1-rc-3" + "version": "v2026.1.1" } ] } From f0575960a3bd8ffdddebdd417e9f6d9c4318d9f3 Mon Sep 17 00:00:00 2001 From: Maxwell Morgan Date: Fri, 23 Jan 2026 18:11:14 -0500 Subject: [PATCH 09/61] Remove unused vendor dep, correct imports --- src/main/java/frc/robot/RobotState.java | 40 +++++++++- .../frc/robot/subsystems/drive/Drive.java | 2 +- .../robot/subsystems/drive/GyroIOPigeon2.java | 2 +- .../subsystems/drive/ModuleIOTalonFX.java | 2 +- .../subsystems/drive/ModuleIOTalonFXS.java | 2 +- .../drive/PhoenixOdometryThread.java | 2 +- vendordeps/StudicaLib.json | 78 ------------------- 7 files changed, 41 insertions(+), 87 deletions(-) delete mode 100644 vendordeps/StudicaLib.json diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index b98beda..36c00af 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -5,6 +5,7 @@ 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.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -12,7 +13,7 @@ import org.littletonrobotics.junction.Logger; public class RobotState { - private static RobotState instance; + private static RobotState instance = new RobotState(); public static RobotState getInstance() { if (instance == null) instance = new RobotState(); @@ -22,6 +23,8 @@ public static RobotState getInstance() { /** Pose Estimator */ private SwerveDrivePoseEstimator poseEstimator; + private ChassisSpeeds robotVelocity; + private RobotState() { poseEstimator = new SwerveDrivePoseEstimator( @@ -36,7 +39,12 @@ private RobotState() { Pose2d.kZero); } - /** Update robot state from drive sensors. */ + /** + * Update robot pose estimate from drive sensors. + * + * @param observation An {@link OdometryObservation} object representing the measured odometry + * state. + */ public void addOdometryObservation(OdometryObservation observation) { // if (observation.gyroAngle().isEmpty()) { @@ -50,6 +58,11 @@ public void addOdometryObservation(OdometryObservation observation) { Logger.recordOutput("RobotState/EstimatedPose", poseEstimator.getEstimatedPosition()); } + /** + * Update robot pose estimate from cameras. + * + * @param measurement A {@link VisionMeasurement} object representing the vision pose estimate. + */ public void addVisionMeasurement(VisionMeasurement measurement) { poseEstimator.addVisionMeasurement( measurement.visionPose().toPose2d(), measurement.timestamp(), measurement.stdDevs()); @@ -57,17 +70,36 @@ public void addVisionMeasurement(VisionMeasurement measurement) { Logger.recordOutput("RobotState/EstimatedPose", poseEstimator.getEstimatedPosition()); } - /** Reset pose estimate and align gyro frame to the given pose. */ + /** + * Reset pose estimate and align gyro frame to the given pose. + * + * @param pose The pose to reset the pose estimator to. + * @param modulePositions An array of the current swerve module positions. + * @param rawGyroRotation The estimated rotation from the drivetrain. + */ public void setPose( Pose2d pose, SwerveModulePosition[] modulePositions, Rotation2d rawGyroRotation) { poseEstimator.resetPosition(rawGyroRotation, modulePositions, pose); } - /** Field-relative estimated robot pose. */ + /** + * Field-relative estimated robot pose. + * + * @return A Pose2d object representing the robot's estimated pose. + */ public Pose2d getEstimatedPose() { return poseEstimator.getEstimatedPosition(); } + /** + * Current robot velocity. + * + * @return A ChassisSpeeds object representing the velocity of the robot. + */ + public ChassisSpeeds getRobotVelocity() { + return robotVelocity; + } + public record OdometryObservation( double timestamp, SwerveModulePosition[] modulePositions, Rotation2d gyroAngle) {} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index d1e58b3..4ca991f 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -27,8 +27,8 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.DriveConstants.ModuleConstants; import frc.robot.Constants.Mode; -import frc.robot.Constants.ModuleConstants; import frc.robot.RobotState; import frc.robot.RobotState.OdometryObservation; import java.util.concurrent.locks.Lock; diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 6dc7a08..9f1fb04 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -17,7 +17,7 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.ModuleConstants; +import frc.robot.Constants.DriveConstants.ModuleConstants; import java.util.Queue; /** IO implementation for Pigeon 2. */ diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 5406907..3d76494 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -35,7 +35,7 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.ModuleConstants; +import frc.robot.Constants.DriveConstants.ModuleConstants; import java.util.Queue; /** diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java index 2fd066e..135ba21 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java @@ -33,7 +33,7 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.ModuleConstants; +import frc.robot.Constants.DriveConstants.ModuleConstants; import java.util.Queue; /** diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 5b87a66..b2ce36b 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -12,7 +12,7 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.RobotController; import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.ModuleConstants; +import frc.robot.Constants.DriveConstants.ModuleConstants; import java.util.ArrayList; import java.util.List; import java.util.Queue; diff --git a/vendordeps/StudicaLib.json b/vendordeps/StudicaLib.json deleted file mode 100644 index 4b4a823..0000000 --- a/vendordeps/StudicaLib.json +++ /dev/null @@ -1,78 +0,0 @@ -{ - "fileName": "StudicaLib.json", - "name": "StudicaLib", - "version": "2026.0.1", - "frcYear": "2026", - "uuid": "963ef341-8abd-4981-a969-c8ae3f592e82", - "mavenUrls": [ - "https://dev.studica.com/maven/release/2026/" - ], - "jsonUrl": "https://dev.studica.com/maven/release/2026/json/StudicaLib-2026.0.1.json", - "conflictsWith": [ - { - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "errorMessage": "StudicaLib (newer devices) is not compatible with Studica (NavX, NavX2)", - "offlineFileName": "Studica.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.studica.frc", - "artifactId": "StudicaLib-java", - "version": "2026.0.1" - } - ], - "jniDependencies": [ - { - "groupId": "com.studica.frc", - "artifactId": "StudicaLib-driver", - "version": "2026.0.1", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.studica.frc", - "artifactId": "StudicaLib-cpp", - "version": "2026.0.1", - "libName": "StudicaLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.studica.frc", - "artifactId": "StudicaLib-driver", - "version": "2026.0.1", - "libName": "StudicaLibDriver", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ] -} From d57942455afb32e49ff16e2cb360740ea2c3839e Mon Sep 17 00:00:00 2001 From: Maxwell Morgan Date: Mon, 26 Jan 2026 18:16:21 -0500 Subject: [PATCH 10/61] Remove unused imports --- src/main/java/frc/robot/Constants.java | 461 +----------------- src/main/java/frc/robot/RobotContainer.java | 22 +- src/main/java/frc/robot/RobotState.java | 4 +- .../frc/robot/subsystems/drive/Drive.java | 3 +- 4 files changed, 30 insertions(+), 460 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0e0954c..3b5af1c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,7 +22,6 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.ctre.phoenix6.swerve.SwerveDrivetrain; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; @@ -34,19 +33,15 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.RobotConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Distance; @@ -54,7 +49,6 @@ import edu.wpi.first.units.measure.MomentOfInertia; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotBase; -import java.util.Map; /** * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running @@ -78,6 +72,9 @@ public static enum Mode { REPLAY } + public static final int kDriverControllerPort = 0; + public static final int kAuxControllerPort = 1; + public static boolean kDisableHAL = false; public static void disableHAL() { @@ -86,29 +83,6 @@ public static void disableHAL() { public static final class DriveConstants { - public static final class ModuleConfigs { - - public static record ModuleConfig( - int driveMotorID, int angleMotorID, int canCoderID, Rotation2d angleOffset) {} - - /** Module 0 (front left) configs. */ - public static final ModuleConfig FrontLeft = - new ModuleConfig(1, 2, 19, Rotation2d.fromDegrees(304.36523 - 180)); - - /** Module 1 (front right) configs. */ - public static final ModuleConfig FrontRight = - new ModuleConfig(2, 4, 20, Rotation2d.fromDegrees(206.455)); - - /** Module 2 (back left) configs. */ - public static final ModuleConfig BackLeft = - new ModuleConfig(5, 6, 21, Rotation2d.fromDegrees(35.419922 + 180)); - - /** Module 3 (back right) configs. */ - public static final ModuleConfig BackRight = - new ModuleConfig(7, 8, 22, Rotation2d.fromDegrees(116.89453)); - } - - // TunerConstants doesn't include these constants public static final double kOdometryFrequency = ModuleConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0; public static final double kDriveBaseRadius = @@ -134,6 +108,7 @@ public static record ModuleConfig( ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY) }; + // TODO: Update for robot // PathPlanner config constants public static final double kRobotMassKG = 74.088; public static final double kRobotMOI = 6.883; @@ -154,114 +129,13 @@ public static record ModuleConfig( 1), kModuleTranslations); - public static final IdleMode kDriveIdleMode = IdleMode.kBrake; - public static final IdleMode kAngleIdleMode = IdleMode.kBrake; - public static final double kDrivePower = 1; - public static final double kAnglePower = .9; - - public static final boolean kInvertGyro = false; // Always ensure Gyro is CCW+ CW- - - // drivetrain constants - public static final double kTrackWidth = Units.inchesToMeters(24.75); - public static final double kWheelBase = Units.inchesToMeters(24.75); - public static final double kWheelDiameter = Units.inchesToMeters(4.0); - public static final double kWheelRadius = kWheelDiameter / 2.0; - public static final double kWheelCircumference = kWheelDiameter * Math.PI; - - // Swerve kinematics, don't change - public static final SwerveDriveKinematics swerveKinematics = - new SwerveDriveKinematics( - new Translation2d(kWheelBase / 2.0, kTrackWidth / 2.0), // front left - new Translation2d(kWheelBase / 2.0, -kTrackWidth / 2.0), // front right - new Translation2d(-kWheelBase / 2.0, kTrackWidth / 2.0), // back left - new Translation2d(-kWheelBase / 2.0, -kTrackWidth / 2.0)); // back right - - // gear ratios - public static final double kDriveGearRatio = (6.12 / 1.0); - public static final double kAngleGearRatio = ((150.0 / 7.0) / 1.0); - - // encoder stuff - // meters per rotation - public static final double kDriveRevToMeters = kWheelCircumference / (kDriveGearRatio); - public static final double kDriveRpmToMetersPerSecond = kDriveRevToMeters / 60; - - /** The number of degrees that a single rotation of the turn motor turns the // wheel. */ - public static final double kDegreesPerTurnRotation = 360 / kAngleGearRatio; - - // motor inverts, check these - public static final boolean kAngleMotorInvert = true; - public static final InvertedValue kDriveMotorInvert = InvertedValue.CounterClockwise_Positive; - - /* Angle Encoder Invert */ - public static final boolean kCanCoderInvert = false; - - /* Swerve Current Limiting */ - public static final int kAngleContinuousCurrentLimit = 20; - public static final int kAnglePeakCurrentLimit = 40; - public static final double kAnglePeakCurrentDuration = 0.1; - public static final boolean kAngleEnableCurrentLimit = true; - - public static final int kDriveSupplyCurrentLimit = 60; - public static final boolean kDriveSupplyCurrentLimitEnable = true; - public static final int kDriveSupplyCurrentThreshold = 60; - public static final double kDriveSupplyTimeThreshold = 0.1; - - public static final boolean kDriveEnableCurrentLimit = true; - - /* - * These values are used by the drive falcon to ramp in open loop and closed - * loop driving. - * We found a small open loop ramp (0.25) helps with tread wear, tipping, etc - */ - public static final double kOpenLoopRamp = 0.25; - public static final double kClosedLoopRamp = 0.0; - - /* Angle Motor PID Values */ - public static final double kAngleKP = 0.015; - public static final double kAngleKI = 0; - public static final double kAngleKD = 0; - public static final double kAngleKF = 0; - - /* Drive Motor PID Values */ - - public static final double kDriveKP = 0.01; - public static final double kDriveKI = 0.0; - public static final double kDriveKD = 0.0; - - public static final double kDriveKS = (0.32 / 12); - public static final double kDriveKV = (1.988 / 12); - public static final double kDriveKA = (1.0449 / 12); - - /* Swerve Profiling Values */ - /** Meters per second. */ - public static final double kPhysicalMaxSpeed = 5.0; - - public static final double kMaxTeleDriveSpeed = 4.5; - /** Radians per second. */ - public static final double kPhysicalMaxAngularSpeed = 2 * 2 * Math.PI; - /** Radians per second. */ - public static final double kMaxTeleAngularSpeed = kPhysicalMaxAngularSpeed / 2; - - public static final double kMaxAngularAccelerationSpeed = 4 / Math.PI; - /** Radians per second. */ - public static final double kMaxTeleAngularAccelerationSpeed = kMaxAngularAccelerationSpeed / 2; - - public static final double kDeadband = 0.08; - - public static final Map kDistances = - Map.of( - 0, 0.0, - 1, 1.0, - 2, 2.0, - 3, 3.0, - 4, 4.0); - public static class ModuleConstants { // Both sets of gains need to be tuned to your individual robot. // The steer motor uses any SwerveModule.SteerRequestType control request with // the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + // TODO: Update for robot private static final Slot0Configs steerGains = new Slot0Configs() .withKP(100) @@ -273,6 +147,7 @@ public static class ModuleConstants { .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + // TODO: Update for robot private static final Slot0Configs driveGains = new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); @@ -299,6 +174,7 @@ public static class ModuleConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot + // TODO: Update for robot private static final Current kSlipCurrent = Amps.of(120.0); // Initial configs for the drive and steer motors and the azimuth encoder; these @@ -327,19 +203,21 @@ public static class ModuleConstants { // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot + // TODO: Update for robot public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot + // TODO: Update for robot private static final double kCoupleRatio = 3.8181818181818183; - + // TODO: Update for robot private static final double kDriveGearRatio = 7.363636363636365; private static final double kSteerGearRatio = 15.42857142857143; private static final Distance kWheelRadius = Inches.of(2.167); - + // TODO: Update for robot private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; - + // TODO: Update for robot private static final int kPigeonId = 1; // These are only used for simulation @@ -381,6 +259,7 @@ public static class ModuleConstants { .withSteerFrictionVoltage(kSteerFrictionVoltage) .withDriveFrictionVoltage(kDriveFrictionVoltage); + // TODO: Update for robot // Front Left private static final int kFrontLeftDriveMotorId = 3; private static final int kFrontLeftSteerMotorId = 2; @@ -391,7 +270,7 @@ public static class ModuleConstants { private static final Distance kFrontLeftXPos = Inches.of(10); private static final Distance kFrontLeftYPos = Inches.of(10); - + // TODO: Update for robot // Front Right private static final int kFrontRightDriveMotorId = 1; private static final int kFrontRightSteerMotorId = 0; @@ -402,7 +281,7 @@ public static class ModuleConstants { private static final Distance kFrontRightXPos = Inches.of(10); private static final Distance kFrontRightYPos = Inches.of(-10); - + // TODO: Update for robot // Back Left private static final int kBackLeftDriveMotorId = 7; private static final int kBackLeftSteerMotorId = 6; @@ -413,7 +292,7 @@ public static class ModuleConstants { private static final Distance kBackLeftXPos = Inches.of(-10); private static final Distance kBackLeftYPos = Inches.of(10); - + // TODO: Update for robot // Back Right private static final int kBackRightDriveMotorId = 5; private static final int kBackRightSteerMotorId = 4; @@ -569,314 +448,6 @@ public TunerSwerveDrivetrain( } } - public class ModuleConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with - // the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = - new Slot0Configs() - .withKP(100) - .withKI(0) - .withKD(0.5) - .withKS(0.1) - .withKV(1.91) - .withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = - new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = - DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = - SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to - // RemoteCANcoder - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); - - // Initial configs for the drive and steer motors and the azimuth encoder; these - // cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API - // documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = - new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a - // relatively - // low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true)); - private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; - - // CAN bus that the devices are located on; - // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot"); - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.8181818181818183; - - private static final double kDriveGearRatio = 7.363636363636365; - private static final double kSteerGearRatio = 15.42857142857143; - private static final Distance kWheelRadius = Inches.of(2.167); - - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - - private static final int kPigeonId = 1; - - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - - public static final SwerveDrivetrainConstants DrivetrainConstants = - new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - ConstantCreator = - new SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - - // Front Left - private static final int kFrontLeftDriveMotorId = 3; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 1; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); - private static final boolean kFrontLeftSteerMotorInverted = true; - private static final boolean kFrontLeftEncoderInverted = false; - - private static final Distance kFrontLeftXPos = Inches.of(10); - private static final Distance kFrontLeftYPos = Inches.of(10); - - // Front Right - private static final int kFrontRightDriveMotorId = 1; - private static final int kFrontRightSteerMotorId = 0; - private static final int kFrontRightEncoderId = 0; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); - private static final boolean kFrontRightSteerMotorInverted = true; - private static final boolean kFrontRightEncoderInverted = false; - - private static final Distance kFrontRightXPos = Inches.of(10); - private static final Distance kFrontRightYPos = Inches.of(-10); - - // Back Left - private static final int kBackLeftDriveMotorId = 7; - private static final int kBackLeftSteerMotorId = 6; - private static final int kBackLeftEncoderId = 3; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); - private static final boolean kBackLeftSteerMotorInverted = true; - private static final boolean kBackLeftEncoderInverted = false; - - private static final Distance kBackLeftXPos = Inches.of(-10); - private static final Distance kBackLeftYPos = Inches.of(10); - - // Back Right - private static final int kBackRightDriveMotorId = 5; - private static final int kBackRightSteerMotorId = 4; - private static final int kBackRightEncoderId = 2; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); - private static final boolean kBackRightSteerMotorInverted = true; - private static final boolean kBackRightEncoderInverted = false; - - private static final Distance kBackRightXPos = Inches.of(-10); - private static final Distance kBackRightYPos = Inches.of(-10); - - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, - kFrontLeftDriveMotorId, - kFrontLeftEncoderId, - kFrontLeftEncoderOffset, - kFrontLeftXPos, - kFrontLeftYPos, - kInvertLeftSide, - kFrontLeftSteerMotorInverted, - kFrontLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, - kFrontRightDriveMotorId, - kFrontRightEncoderId, - kFrontRightEncoderOffset, - kFrontRightXPos, - kFrontRightYPos, - kInvertRightSide, - kFrontRightSteerMotorInverted, - kFrontRightEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, - kBackLeftDriveMotorId, - kBackLeftEncoderId, - kBackLeftEncoderOffset, - kBackLeftXPos, - kBackLeftYPos, - kInvertLeftSide, - kBackLeftSteerMotorInverted, - kBackLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, - kBackRightDriveMotorId, - kBackRightEncoderId, - kBackRightEncoderOffset, - kBackRightXPos, - kBackRightYPos, - kInvertRightSide, - kBackRightSteerMotorInverted, - kBackRightEncoderInverted); - - /** - * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot - * program,. - */ - // public static CommandSwerveDrivetrain createDrivetrain() { - // return new CommandSwerveDrivetrain( - // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); - // } - - /** - * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. - */ - public static class TunerSwerveDrivetrain extends SwerveDrivetrain { - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the - * devices themselves. If they need the devices, they can access them through getters in the - * classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules) { - super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the - * devices themselves. If they need the devices, they can access them through getters in the - * classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or - * set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules) { - super( - TalonFX::new, - TalonFX::new, - CANcoder::new, - drivetrainConstants, - odometryUpdateFrequency, - modules); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the - * devices themselves. If they need the devices, they can access them through getters in the - * classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or - * set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation in the - * form [x, y, theta]áµ€, with units in meters and radians - * @param visionStandardDeviation The standard deviation for vision calculation in the form - * [x, y, theta]áµ€, with units in meters and radians - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules) { - super( - TalonFX::new, - TalonFX::new, - CANcoder::new, - drivetrainConstants, - odometryUpdateFrequency, - odometryStandardDeviation, - visionStandardDeviation, - modules); - } - } - } - public class VisionConstants { // AprilTag layout public static AprilTagFieldLayout aprilTagLayout = diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f1b07c5..0ec423b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj.Timer; 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.CommandPS5Controller; import frc.robot.Constants.DriveConstants.ModuleConstants; import frc.robot.RobotState.OdometryObservation; import frc.robot.commands.DriveCommands; @@ -20,17 +20,16 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; -import frc.robot.subsystems.vision.CameraIO; -import frc.robot.subsystems.vision.Vision; import frc.robot.util.AllianceFlipUtil; import frc.robot.util.FieldConstants; import frc.robot.util.FieldConstants.Hub; public class RobotContainer { - private final CommandXboxController driver = new CommandXboxController(0); + private final CommandPS5Controller driver = + new CommandPS5Controller(Constants.kDriverControllerPort); private final Drive drive; - private final Vision vision; + // private final Vision vision; public RobotContainer() { switch (Constants.kCurrentMode) { @@ -42,7 +41,7 @@ public RobotContainer() { new ModuleIOTalonFX(ModuleConstants.FrontRight), new ModuleIOTalonFX(ModuleConstants.BackLeft), new ModuleIOTalonFX(ModuleConstants.BackRight)); - vision = new Vision(null, null); + // vision = new Vision(null, null); break; case SIM: drive = @@ -52,7 +51,7 @@ public RobotContainer() { new ModuleIOSim(ModuleConstants.FrontRight), new ModuleIOSim(ModuleConstants.BackLeft), new ModuleIOSim(ModuleConstants.BackRight)); - vision = new Vision(null, null); + // vision = new Vision(null, null); break; case REPLAY: default: @@ -63,7 +62,7 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); - vision = new Vision(null, new CameraIO[] {}); + // vision = new Vision(null, new CameraIO[] {}); break; } @@ -76,7 +75,7 @@ private void configureBindings() { drive, () -> -driver.getLeftY(), () -> -driver.getLeftX(), () -> -driver.getRightX())); driver - .rightBumper() + .R1() .whileTrue( DriveCommands.joystickDriveAtAngle( drive, @@ -89,12 +88,11 @@ private void configureBindings() { Translation2d delta = target.minus(robotPose.getTranslation()); - return new Rotation2d(Math.atan2(delta.getY(), delta.getX())) - .plus(Rotation2d.k180deg); // Because KitBot shooter is on the back + return new Rotation2d(Math.atan2(delta.getY(), delta.getX())); })); driver - .y() + .triangle() .onTrue( DriveCommands.turnToPoint( drive, diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 36c00af..7a9da6f 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -9,7 +9,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import frc.robot.Constants.DriveConstants; +import frc.robot.subsystems.drive.Drive; import org.littletonrobotics.junction.Logger; public class RobotState { @@ -28,7 +28,7 @@ public static RobotState getInstance() { private RobotState() { poseEstimator = new SwerveDrivePoseEstimator( - DriveConstants.swerveKinematics, + Drive.kinematics, Rotation2d.kZero, new SwerveModulePosition[] { new SwerveModulePosition(), diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 4ca991f..1cc5156 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -45,7 +45,8 @@ public class Drive extends SubsystemBase { private final Alert gyroDisconnectedAlert = new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); + public static final SwerveDriveKinematics kinematics = + new SwerveDriveKinematics(getModuleTranslations()); private Rotation2d rawGyroRotation = Rotation2d.kZero; private SwerveModulePosition[] lastModulePositions = // For delta tracking new SwerveModulePosition[] { From 518791c62f36f906340570d4c5201aa45e5e2062 Mon Sep 17 00:00:00 2001 From: Maxwell Morgan Date: Mon, 26 Jan 2026 18:20:50 -0500 Subject: [PATCH 11/61] Remove unneeded methods, reset hardware file --- .../frc/robot/subsystems/intake/IntakeIO.java | 3 -- .../subsystems/intake/IntakeIOHardware.java | 10 +++++++ .../subsystems/intake/IntakeIOSparkMax.java | 30 ------------------- 3 files changed, 10 insertions(+), 33 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java delete mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index d9250b4..955a0df 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -17,7 +17,4 @@ default void updateInputs(IntakeIOInputs inputs) {} @AutoLog public class IntakeIOInputs {} - default void runPivotMotor(double speed) {} - - default void runRollerMotor(double speed) {} } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java new file mode 100644 index 0000000..0d27a60 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java @@ -0,0 +1,10 @@ +package frc.robot.subsystems.intake; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import frc.robot.Constants.IntakeConstants; + +public class IntakeIOHardware implements IntakeIO { + +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java deleted file mode 100644 index 69dd1a9..0000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java +++ /dev/null @@ -1,30 +0,0 @@ -package frc.robot.subsystems.intake; - -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; -import frc.robot.Constants.IntakeConstants; - -public class IntakeIOSparkMax implements IntakeIO { - - private SparkMax pivotMotor = new SparkMax(IntakeConstants.kPivotMotorID, MotorType.kBrushless); - private SparkMax rollerMotor = new SparkMax(IntakeConstants.kRollerMotorID, MotorType.kBrushless); - - private RelativeEncoder pivotEncoder = pivotMotor.getEncoder(); - private RelativeEncoder rollerEncoder = rollerMotor.getEncoder(); - - public IntakeIOSparkMax() {} - - @Override - public void updateInputs(IntakeIOInputs inputs) {} - - @Override - public void runPivotMotor(double speed) { - pivotMotor.set(speed); - } - - @Override - public void runRollerMotor(double speed) { - rollerMotor.set(speed); - } -} From d5a1c6682180ea3e1cb3d1381bffdd8cc1f86647 Mon Sep 17 00:00:00 2001 From: Maxwell Morgan Date: Mon, 26 Jan 2026 18:27:18 -0500 Subject: [PATCH 12/61] Create guts folder and skeleton classes --- .../java/frc/robot/subsystems/guts/Guts.java | 17 +++++++++++++++++ .../java/frc/robot/subsystems/guts/GutsIO.java | 10 ++++++++++ .../frc/robot/subsystems/guts/GutsIOSim.java | 3 +++ .../robot/subsystems/guts/GutsIOTalonFX.java | 3 +++ 4 files changed, 33 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/guts/Guts.java create mode 100644 src/main/java/frc/robot/subsystems/guts/GutsIO.java create mode 100644 src/main/java/frc/robot/subsystems/guts/GutsIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/guts/GutsIOTalonFX.java diff --git a/src/main/java/frc/robot/subsystems/guts/Guts.java b/src/main/java/frc/robot/subsystems/guts/Guts.java new file mode 100644 index 0000000..3809179 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/guts/Guts.java @@ -0,0 +1,17 @@ +// 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.guts; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Guts extends SubsystemBase { + /** Creates a new Guts. */ + public Guts() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIO.java b/src/main/java/frc/robot/subsystems/guts/GutsIO.java new file mode 100644 index 0000000..68f158c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/guts/GutsIO.java @@ -0,0 +1,10 @@ +package frc.robot.subsystems.guts; + +import org.littletonrobotics.junction.AutoLog; + +public interface GutsIO { + default void updateInputs(GutsIOInputs inputs) {} + + @AutoLog + public static class GutsIOInputs {} +} diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIOSim.java b/src/main/java/frc/robot/subsystems/guts/GutsIOSim.java new file mode 100644 index 0000000..eb6cb5b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/guts/GutsIOSim.java @@ -0,0 +1,3 @@ +package frc.robot.subsystems.guts; + +public class GutsIOSim implements GutsIO {} diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIOTalonFX.java b/src/main/java/frc/robot/subsystems/guts/GutsIOTalonFX.java new file mode 100644 index 0000000..433fd18 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/guts/GutsIOTalonFX.java @@ -0,0 +1,3 @@ +package frc.robot.subsystems.guts; + +public class GutsIOTalonFX implements GutsIO {} From a4045f4d67c76b183cbedd038aede9c3b9da0e3d Mon Sep 17 00:00:00 2001 From: Maxwell Morgan Date: Mon, 26 Jan 2026 18:29:35 -0500 Subject: [PATCH 13/61] Update intake skeleton classes --- src/main/java/frc/robot/subsystems/intake/IntakeIO.java | 3 +-- .../frc/robot/subsystems/intake/IntakeIOHardware.java | 9 +-------- 2 files changed, 2 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 955a0df..35c4338 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -15,6 +15,5 @@ public interface IntakeIO { default void updateInputs(IntakeIOInputs inputs) {} @AutoLog - public class IntakeIOInputs {} - + public static class IntakeIOInputs {} } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java index 0d27a60..3142379 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java @@ -1,10 +1,3 @@ package frc.robot.subsystems.intake; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; -import frc.robot.Constants.IntakeConstants; - -public class IntakeIOHardware implements IntakeIO { - -} +public class IntakeIOHardware implements IntakeIO {} From fcbad8346ad1fbd31580d59444751b55b869c500 Mon Sep 17 00:00:00 2001 From: Maxwell Morgan Date: Fri, 30 Jan 2026 17:26:59 -0500 Subject: [PATCH 14/61] Add Turret and Hood sim and default classes --- src/main/java/frc/robot/Constants.java | 61 +++++- src/main/java/frc/robot/Robot.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 19 +- src/main/java/frc/robot/RobotState.java | 13 +- src/main/java/frc/robot/RobotVisualizer.java | 135 ++++++++++++++ .../frc/robot/subsystems/drive/Drive.java | 4 +- .../frc/robot/subsystems/shooter/Shooter.java | 55 ++++++ .../shooter/TrajectoryCalculator.java | 8 + .../robot/subsystems/shooter/hood/Hood.java | 19 ++ .../robot/subsystems/shooter/hood/HoodIO.java | 18 ++ .../subsystems/shooter/hood/HoodIOSim.java | 40 ++++ .../shooter/hood/HoodIOSparkMax.java | 3 + .../subsystems/shooter/turret/Turret.java | 84 +++++++++ .../subsystems/shooter/turret/TurretIO.java | 19 ++ .../shooter/turret/TurretIOSim.java | 43 +++++ .../shooter/turret/TurretIOSparkMax.java | 12 ++ src/main/java/frc/robot/util/GeomUtil.java | 174 ++++++++++++++++++ 17 files changed, 702 insertions(+), 9 deletions(-) create mode 100644 src/main/java/frc/robot/RobotVisualizer.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/Shooter.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/hood/Hood.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/hood/HoodIO.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/turret/Turret.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java create mode 100644 src/main/java/frc/robot/util/GeomUtil.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3b5af1c..9b6b5bd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -10,6 +10,7 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.Volts; @@ -39,6 +40,7 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.system.plant.DCMotor; @@ -49,6 +51,8 @@ import edu.wpi.first.units.measure.MomentOfInertia; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotBase; +import frc.robot.subsystems.drive.Drive; +import frc.robot.util.GeomUtil; /** * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running @@ -82,6 +86,8 @@ public static void disableHAL() { } public static final class DriveConstants { + public static final SwerveDriveKinematics kSwerveKinematics = + new SwerveDriveKinematics(Drive.getModuleTranslations()); public static final double kOdometryFrequency = ModuleConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0; @@ -129,7 +135,7 @@ public static final class DriveConstants { 1), kModuleTranslations); - public static class ModuleConstants { + public static final class ModuleConstants { // Both sets of gains need to be tuned to your individual robot. // The steer motor uses any SwerveModule.SteerRequestType control request with @@ -448,7 +454,7 @@ public TunerSwerveDrivetrain( } } - public class VisionConstants { + public static final class VisionConstants { // AprilTag layout public static AprilTagFieldLayout aprilTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); @@ -486,4 +492,55 @@ public class VisionConstants { public static double angularStdDevMegatag2Factor = Double.POSITIVE_INFINITY; // No rotation data available } + + public static final class ShooterConstants { + + public static final class TurretConstants { + public static final double kGearRatio = 10 / 1; + public static final double kMinTurretAngleRad = -3.0 * Math.PI / 2.0; // -270 degrees + public static final double kMaxTurretAngleRad = 3.0 * Math.PI / 2.0; // +270 degrees + + public static final double kLeftMotorId = 12; + public static final double kRightMotorId = 13; + + // +X = Forward, +Y = Left + public static final Transform3d kRobotToLeftTurret = + new Transform3d(Inches.of(3.749), Inches.of(8.186), Inches.of(13.401), Rotation3d.kZero); + + public static final Transform3d kRobotToRightTurret = + new Transform3d(Inches.of(3.749), Inches.of(-8.314), Inches.of(13.401), Rotation3d.kZero); + } + + public static final class HoodConstants { + public static final double kTurretToHoodInches = 1.878; + public static final double kGearRatio = 100 / 1; + + public static final Transform3d kRobotToLeftHood = + new Transform3d( + Inches.of(7.268715), Meters.of(0.20792316), Inches.of(16.018516), Rotation3d.kZero); + + public static final Transform3d kRobotToRightHood = + new Transform3d( + Inches.of(-7.270121), + Inches.of(-(12.062888 - (7.5 / 2.0))), + Inches.of(16.018516), + Rotation3d.kZero); + + public static final Transform3d kLeftTurretToLeftHood = + GeomUtil.toPose3d(HoodConstants.kRobotToLeftHood) + .minus( + GeomUtil.toPose3d(TurretConstants.kRobotToLeftTurret) + .plus( + new Transform3d( + Inches.of(7.268715), Inches.of(0), Inches.of(0), new Rotation3d()))); + + public static final Transform3d kRightTurretToRightHood = + GeomUtil.toPose3d(HoodConstants.kRobotToRightHood) + .minus( + GeomUtil.toPose3d(TurretConstants.kRobotToRightTurret) + .plus( + new Transform3d( + Inches.of(-7.270121), Inches.of(0), Inches.of(0), new Rotation3d()))); + } + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a95fa00..ded80a4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -77,6 +77,8 @@ public void robotPeriodic() { CachedSupplier.invalidateAll(); robotContainer.robotPeriodic(); CommandScheduler.getInstance().run(); + + RobotVisualizer.getInstance().log("Mechanism3d/Robot"); } /** This function is called once when the robot is disabled. */ @@ -93,7 +95,7 @@ public void autonomousInit() { autonomousCommand = robotContainer.getAutonomousCommand(); if (autonomousCommand != null) { - autonomousCommand.schedule(); + CommandScheduler.getInstance().schedule(autonomousCommand); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0ec423b..6ae50fb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,6 +20,9 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; +import frc.robot.subsystems.shooter.Shooter.ShooterSide; +import frc.robot.subsystems.shooter.turret.Turret; +import frc.robot.subsystems.shooter.turret.TurretIOSim; import frc.robot.util.AllianceFlipUtil; import frc.robot.util.FieldConstants; import frc.robot.util.FieldConstants.Hub; @@ -28,8 +31,10 @@ public class RobotContainer { private final CommandPS5Controller driver = new CommandPS5Controller(Constants.kDriverControllerPort); - private final Drive drive; - // private final Vision vision; + private Drive drive; + private Turret leftShooter; + private Turret rightShooter; + // private Vision vision; public RobotContainer() { switch (Constants.kCurrentMode) { @@ -51,6 +56,8 @@ public RobotContainer() { new ModuleIOSim(ModuleConstants.FrontRight), new ModuleIOSim(ModuleConstants.BackLeft), new ModuleIOSim(ModuleConstants.BackRight)); + leftShooter = new Turret(ShooterSide.LEFT, new TurretIOSim()); + rightShooter = new Turret(ShooterSide.RIGHT, new TurretIOSim()); // vision = new Vision(null, null); break; case REPLAY: @@ -73,6 +80,14 @@ private void configureBindings() { drive.setDefaultCommand( DriveCommands.joystickDrive( drive, () -> -driver.getLeftY(), () -> -driver.getLeftX(), () -> -driver.getRightX())); + leftShooter.setDefaultCommand( + leftShooter.trackTarget( + () -> RobotState.getInstance().getEstimatedPose(), + () -> AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d()))); + rightShooter.setDefaultCommand( + rightShooter.trackTarget( + () -> RobotState.getInstance().getEstimatedPose(), + () -> AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d()))); driver .R1() diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 7a9da6f..3b7d9f5 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -9,7 +9,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import frc.robot.subsystems.drive.Drive; +import frc.robot.Constants.DriveConstants; import org.littletonrobotics.junction.Logger; public class RobotState { @@ -28,7 +28,7 @@ public static RobotState getInstance() { private RobotState() { poseEstimator = new SwerveDrivePoseEstimator( - Drive.kinematics, + DriveConstants.kSwerveKinematics, Rotation2d.kZero, new SwerveModulePosition[] { new SwerveModulePosition(), @@ -82,6 +82,15 @@ public void setPose( poseEstimator.resetPosition(rawGyroRotation, modulePositions, pose); } + /** + * Set the robot's velocity + * + * @param speeds A ChassisSpeeds object representing the robot's current velocity + */ + public void setRobotVelocity(ChassisSpeeds speeds) { + robotVelocity = speeds; + } + /** * Field-relative estimated robot pose. * diff --git a/src/main/java/frc/robot/RobotVisualizer.java b/src/main/java/frc/robot/RobotVisualizer.java new file mode 100644 index 0000000..313df35 --- /dev/null +++ b/src/main/java/frc/robot/RobotVisualizer.java @@ -0,0 +1,135 @@ +package frc.robot; + +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.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants.ShooterConstants.HoodConstants; +import frc.robot.Constants.ShooterConstants.TurretConstants; +import frc.robot.util.GeomUtil; +import org.littletonrobotics.junction.Logger; + +public class RobotVisualizer { + private static RobotVisualizer instance; + + public static RobotVisualizer getInstance() { + if (instance == null) { + instance = new RobotVisualizer(); + } + return instance; + } + + private Rotation2d[] turretAngles = {Rotation2d.kZero, Rotation2d.kZero}; // Left, Right + private double[] hoodAngles = {0.0, 0.0}; // Left, Right + + private RobotVisualizer() {} + + /** + * Logs the component poses. + * + * @param key A String representing the output location. + */ + public void log(String key) { + Pose3d leftTurretPose = + GeomUtil.toPose3d(TurretConstants.kRobotToLeftTurret) + .transformBy( + new Transform3d( + Translation3d.kZero, + new Rotation3d(0.0, 0.0, turretAngles[0].plus(Rotation2d.kPi).getRadians()))); + Pose3d rightTurretPose = + GeomUtil.toPose3d(TurretConstants.kRobotToRightTurret) + .transformBy( + new Transform3d( + Translation3d.kZero, + new Rotation3d(0.0, 0.0, turretAngles[1].plus(Rotation2d.kPi).getRadians()))); + + Pose3d leftHoodPose = + leftTurretPose.transformBy( + new Transform3d( + HoodConstants.kLeftTurretToLeftHood.getTranslation(), + new Rotation3d(0.0, Math.abs(Math.sin(Timer.getFPGATimestamp())) * -0.5, 0.0))); + + Pose3d rightHoodPose = + rightTurretPose.transformBy( + new Transform3d( + HoodConstants.kRightTurretToRightHood.getTranslation(), + new Rotation3d(0.0, Math.abs(Math.sin(Timer.getFPGATimestamp())) * -0.5, 0.0))); + + Logger.recordOutput( + key + "/Components", leftTurretPose, rightTurretPose, leftHoodPose, rightHoodPose); + } + + /** + * Gets the left turret angle. + * + * @return A Rotation2d object representing the left turret angle. + */ + public Rotation2d getLeftTurretAngle() { + return turretAngles[0]; + } + + /** + * Gets the right turret angle. + * + * @return A Rotation2d object representing the right turret angle. + */ + public Rotation2d getRightTurretAngle() { + return turretAngles[1]; + } + + /** + * Sets the left turret angle. + * + * @param angle A Rotation2d object to be inserted in the angles array. + */ + public void setLeftTurretAngle(Rotation2d angle) { + turretAngles[0] = angle; + } + + /** + * Sets the right turret angle. + * + * @param angle A Rotation2d object to be inserted in the angles array. + */ + public void setRightTurretAngle(Rotation2d angle) { + turretAngles[1] = angle; + } + + /** + * Gets the left hood angle. + * + * @return A double representing the left hood angle in radians. + */ + public double getLeftHoodAngle() { + return hoodAngles[0]; + } + + /** + * Gets the left hood angle. + * + * @return A double representing the right hood angle in radians. + */ + public double getRightHoodAngle() { + return hoodAngles[1]; + } + + /** + * Sets the left hood angle in radians. + * + * @param angle A Rotation2d object to be inserted in the angles array. + */ + public void setLeftTurretAngle(double angle) { + hoodAngles[0] = angle; + } + + /** + * Sets the right hood angle in radians. + * + * @param angle A double to be inserted in the angles array. + */ + public void setLeftHoodAngle(double angle) { + hoodAngles[1] = angle; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 1cc5156..1765c09 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -45,8 +45,8 @@ public class Drive extends SubsystemBase { private final Alert gyroDisconnectedAlert = new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); - public static final SwerveDriveKinematics kinematics = - new SwerveDriveKinematics(getModuleTranslations()); + private final SwerveDriveKinematics kinematics = DriveConstants.kSwerveKinematics; + private Rotation2d rawGyroRotation = Rotation2d.kZero; private SwerveModulePosition[] lastModulePositions = // For delta tracking new SwerveModulePosition[] { diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java new file mode 100644 index 0000000..68cb95a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -0,0 +1,55 @@ +// 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.shooter; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.shooter.hood.Hood; +import frc.robot.subsystems.shooter.hood.HoodIO; +import frc.robot.subsystems.shooter.turret.Turret; +import frc.robot.subsystems.shooter.turret.TurretIO; +import java.util.function.Supplier; + +public class Shooter extends SubsystemBase { + private final ShooterSide side; + + private final Turret turret; + private final Hood hood; + + /** Creates a new Shooter. */ + public Shooter(ShooterSide side, TurretIO turretIO, HoodIO hoodIO) { + this.side = side; + this.turret = new Turret(side, turretIO); + this.hood = new Hood(side, hoodIO); + } + + @Override + public void periodic() { + turret.periodic(); + hood.periodic(); + } + + public Command trackTarget( + Supplier robotPoseSupplier, Supplier targetSupplier) { + return turret.trackTarget(robotPoseSupplier, targetSupplier); + } + + public enum ShooterSide { + LEFT("Left"), + RIGHT("Right"); + + private String name; + + private ShooterSide(String name) { + this.name = name; + } + + public String getName() { + return name; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java new file mode 100644 index 0000000..c60fd7c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java @@ -0,0 +1,8 @@ +package frc.robot.subsystems.shooter; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class TrajectoryCalculator { + + public record ShooterParams(double wheelRPM, double hoodAngle, Rotation2d turretAngle) {} +} diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java b/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java new file mode 100644 index 0000000..5cc4024 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java @@ -0,0 +1,19 @@ +// 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.shooter.hood; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.shooter.Shooter.ShooterSide; + +public class Hood extends SubsystemBase { + + /** Creates a new Hood. */ + public Hood(ShooterSide side, HoodIO io) {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIO.java new file mode 100644 index 0000000..4549488 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIO.java @@ -0,0 +1,18 @@ +package frc.robot.subsystems.shooter.hood; + +import org.littletonrobotics.junction.AutoLog; + +public interface HoodIO { + default void updateInputs(HoodIOInputs inputs) {} + + @AutoLog + public static class HoodIOInputs { + boolean connected = false; + double angleRads = 0.0; + double velocityRadsPerSec = 0.0; + double appliedVolts = 0.0; + double currentAmps = 0.0; + } + + default void setAngle(double angle) {} +} diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSim.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSim.java new file mode 100644 index 0000000..6da98c8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSim.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems.shooter.hood; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants.ShooterConstants.HoodConstants; +import frc.robot.subsystems.shooter.hood.HoodIO.HoodIOInputs; + +public class HoodIOSim implements HoodIO { + private final DCMotor gearbox = DCMotor.getNeo550(1); + private final DCMotorSim sim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(gearbox, 0.025, HoodConstants.kGearRatio), gearbox); + + private PIDController pid = new PIDController(1, 0, 0); + + public HoodIOSim() {} + + @Override + public void updateInputs(HoodIOInputs inputs) { + double currentOutput = pid.calculate(sim.getAngularPositionRad() / HoodConstants.kGearRatio); + double volts = MathUtil.clamp(currentOutput, -12.0, 12.0); + + sim.setInputVoltage(volts); + sim.update(0.02); + + inputs.connected = true; + inputs.angleRads = sim.getAngularPositionRad(); + inputs.velocityRadsPerSec = sim.getAngularVelocityRadPerSec(); + inputs.appliedVolts = volts; + inputs.currentAmps = sim.getCurrentDrawAmps(); + } + + @Override + public void setAngle(double angle) { + pid.setSetpoint(angle); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java new file mode 100644 index 0000000..a315670 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java @@ -0,0 +1,3 @@ +package frc.robot.subsystems.shooter.hood; + +public class HoodIOSparkMax {} diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java new file mode 100644 index 0000000..9a2c316 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java @@ -0,0 +1,84 @@ +// 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.shooter.turret; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.ShooterConstants.TurretConstants; +import frc.robot.RobotVisualizer; +import frc.robot.subsystems.shooter.Shooter.ShooterSide; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; + +public class Turret extends SubsystemBase { + private final ShooterSide side; + + private final TurretIO io; + private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged(); + + private Rotation2d targetAngle = Rotation2d.kZero; + + /** Creates a new Turret. */ + public Turret(ShooterSide side, TurretIO io) { + this.side = side; + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs(("Turret/" + side.getName()), inputs); + + if (side.equals(ShooterSide.LEFT)) { + RobotVisualizer.getInstance().setLeftTurretAngle(Rotation2d.fromRadians(inputs.positionRads)); + } else if (side.equals(ShooterSide.RIGHT)) { + RobotVisualizer.getInstance() + .setRightTurretAngle(Rotation2d.fromRadians(inputs.positionRads)); + } + + Logger.recordOutput(("Turret/" + side.getName() + "/TargetAngle"), targetAngle); + } + + public Command trackTarget( + Supplier robotPoseSupplier, Supplier targetSupplier) { + + return Commands.run( + () -> { + Translation2d target = targetSupplier.get(); + Pose2d robotPose = robotPoseSupplier.get(); + + Translation2d turretOffset = + (this.side == ShooterSide.LEFT + ? TurretConstants.kRobotToLeftTurret.getTranslation().toTranslation2d() + : TurretConstants.kRobotToRightTurret.getTranslation().toTranslation2d()); + + // Turret position in field coordinates + Translation2d turretFieldPos = + robotPose.getTranslation().plus(turretOffset.rotateBy(robotPose.getRotation())); + + // Vector from turret -> target (field frame) + Translation2d deltaField = target.minus(turretFieldPos); + + // Convert to robot frame + Translation2d deltaRobot = deltaField.rotateBy(robotPose.getRotation().unaryMinus()); + + // Angle turret should point (robot-relative) + Rotation2d targetAngle = new Rotation2d(Math.atan2(deltaRobot.getY(), deltaRobot.getX())); + + this.targetAngle = targetAngle; + + io.setPosition(targetAngle); + }, + this); + } + + public double getPosition() { + return inputs.positionRads; + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java new file mode 100644 index 0000000..0c54930 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java @@ -0,0 +1,19 @@ +package frc.robot.subsystems.shooter.turret; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface TurretIO { + public default void updateInputs(TurretIOInputs inputs) {} + + @AutoLog + public static class TurretIOInputs { + boolean connected = false; + double positionRads = 0.0; + double velocityRadsPerSec = 0.0; + double appliedVolts = 0.0; + double currentAmps = 0.0; + } + + public default void setPosition(Rotation2d position) {} +} diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSim.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSim.java new file mode 100644 index 0000000..b545c83 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSim.java @@ -0,0 +1,43 @@ +package frc.robot.subsystems.shooter.turret; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants.ShooterConstants.TurretConstants; + +public class TurretIOSim implements TurretIO { + private final DCMotor gearbox = DCMotor.getNEO(1); + private final DCMotorSim sim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(gearbox, 0.025, TurretConstants.kGearRatio), gearbox); + + private PIDController pid = new PIDController(8, 0, 0.3); + + public TurretIOSim() { + pid.reset(); + pid.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void updateInputs(TurretIOInputs inputs) { + double currentOutput = pid.calculate(sim.getAngularPositionRad()); + double volts = MathUtil.clamp(currentOutput, -12.0, 12.0); + + sim.setInputVoltage(volts); + sim.update(0.02); + + inputs.connected = true; + inputs.positionRads = sim.getAngularPositionRad(); + inputs.velocityRadsPerSec = sim.getAngularVelocityRadPerSec(); + inputs.appliedVolts = volts; + inputs.currentAmps = sim.getCurrentDrawAmps(); + } + + @Override + public void setPosition(Rotation2d position) { + pid.setSetpoint(position.getRadians()); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java new file mode 100644 index 0000000..8ff55c0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java @@ -0,0 +1,12 @@ +package frc.robot.subsystems.shooter.turret; + +public class TurretIOSparkMax implements TurretIO { + + public TurretIOSparkMax(int id) {} + + @Override + public void updateInputs(TurretIOInputs inputs) { + // TODO Auto-generated method stub + TurretIO.super.updateInputs(inputs); + } +} diff --git a/src/main/java/frc/robot/util/GeomUtil.java b/src/main/java/frc/robot/util/GeomUtil.java new file mode 100644 index 0000000..36ab47e --- /dev/null +++ b/src/main/java/frc/robot/util/GeomUtil.java @@ -0,0 +1,174 @@ +// Copyright (c) 2025-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.util; + +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.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +/** Geometry utilities for working with translations, rotations, transforms, and poses. */ +public class GeomUtil { + /** + * Creates a pure translating transform + * + * @param translation The translation to create the transform with + * @return The resulting transform + */ + public static Transform2d toTransform2d(Translation2d translation) { + return new Transform2d(translation, Rotation2d.kZero); + } + + /** + * Creates a pure translating transform + * + * @param x The x coordinate of the translation + * @param y The y coordinate of the translation + * @return The resulting transform + */ + public static Transform2d toTransform2d(double x, double y) { + return new Transform2d(x, y, Rotation2d.kZero); + } + + /** + * Creates a pure rotating transform + * + * @param rotation The rotation to create the transform with + * @return The resulting transform + */ + public static Transform2d toTransform2d(Rotation2d rotation) { + return new Transform2d(Translation2d.kZero, rotation); + } + + /** + * Converts a Pose2d to a Transform2d to be used in a kinematic chain + * + * @param pose The pose that will represent the transform + * @return The resulting transform + */ + public static Transform2d toTransform2d(Pose2d pose) { + return new Transform2d(pose.getTranslation(), pose.getRotation()); + } + + public static Pose2d inverse(Pose2d pose) { + Rotation2d rotationInverse = pose.getRotation().unaryMinus(); + return new Pose2d( + pose.getTranslation().unaryMinus().rotateBy(rotationInverse), rotationInverse); + } + + /** + * Converts a Transform2d to a Pose2d to be used as a position or as the start of a kinematic + * chain + * + * @param transform The transform that will represent the pose + * @return The resulting pose + */ + public static Pose2d toPose2d(Transform2d transform) { + return new Pose2d(transform.getTranslation(), transform.getRotation()); + } + + /** + * Creates a pure translated pose + * + * @param translation The translation to create the pose with + * @return The resulting pose + */ + public static Pose2d toPose2d(Translation2d translation) { + return new Pose2d(translation, Rotation2d.kZero); + } + + /** + * Creates a pure rotated pose + * + * @param rotation The rotation to create the pose with + * @return The resulting pose + */ + public static Pose2d toPose2d(Rotation2d rotation) { + return new Pose2d(Translation2d.kZero, rotation); + } + + /** + * Multiplies a twist by a scaling factor + * + * @param twist The twist to multiply + * @param factor The scaling factor for the twist components + * @return The new twist + */ + public static Twist2d multiply(Twist2d twist, double factor) { + return new Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor); + } + + /** + * Converts a Pose3d to a Transform3d to be used in a kinematic chain + * + * @param pose The pose that will represent the transform + * @return The resulting transform + */ + public static Transform3d toTransform3d(Pose3d pose) { + return new Transform3d(pose.getTranslation(), pose.getRotation()); + } + + /** + * Converts a Transform3d to a Transform2d + * + * @param transform The original transform + * @return The resulting transform + */ + public static Transform2d toTransform2d(Transform3d transform) { + return new Transform2d( + transform.getTranslation().toTranslation2d(), transform.getRotation().toRotation2d()); + } + + /** + * Converts a Transform3d to a Pose3d to be used as a position or as the start of a kinematic + * chain + * + * @param transform The transform that will represent the pose + * @return The resulting pose + */ + public static Pose3d toPose3d(Transform3d transform) { + return new Pose3d(transform.getTranslation(), transform.getRotation()); + } + + /** + * Converts a ChassisSpeeds to a Twist2d by extracting two dimensions (Y and Z). chain + * + * @param speeds The original translation + * @return The resulting translation + */ + public static Twist2d toTwist2d(ChassisSpeeds speeds) { + return new Twist2d( + speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond); + } + + /** + * Creates a new pose from an existing one using a different translation value. + * + * @param pose The original pose + * @param translation The new translation to use + * @return The new pose with the new translation and original rotation + */ + public static Pose2d withTranslation(Pose2d pose, Translation2d translation) { + return new Pose2d(translation, pose.getRotation()); + } + + /** + * Creates a new pose from an existing one using a different rotation value. + * + * @param pose The original pose + * @param rotation The new rotation to use + * @return The new pose with the original translation and new rotation + */ + public static Pose2d withRotation(Pose2d pose, Rotation2d rotation) { + return new Pose2d(pose.getTranslation(), rotation); + } +} From 68ff570707744491506ccb88da54b66d2216e6a9 Mon Sep 17 00:00:00 2001 From: Matthew McGrath Date: Wed, 4 Feb 2026 19:57:35 -0500 Subject: [PATCH 15/61] start the io layers for the intake, including methods and inputs --- .../frc/robot/subsystems/intake/IntakeIO.java | 17 ++++++-- .../subsystems/intake/IntakeIOHardware.java | 41 ++++++++++++++++++- 2 files changed, 54 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 35c4338..edbd4de 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -3,7 +3,8 @@ import org.littletonrobotics.junction.AutoLog; /** - * The {@code IntakeIO} class provides methods for interacting with the intake motors and updating + * The {@code IntakeIO} class provides methods for interacting with the intake + * motors and updating * the intake inputs. * * @author Ryan Hefferon @@ -12,8 +13,18 @@ * @author Julien Precourt */ public interface IntakeIO { - default void updateInputs(IntakeIOInputs inputs) {} + default void updateInputs(IntakeIOInputs inputs) { + } @AutoLog - public static class IntakeIOInputs {} + public static class IntakeIOInputs { + double armMotorVelocityRPM = 0.0; + double wheelMotorVelocityRPM = 0.0; + double armMotorPositionsRotations = 0.0; + double wheelMotorPositionRotations = 0.0; + + } + + default void setArmSpeed(double speed){} + default void setWheelSpeed(double speed){} } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java index 3142379..b2f2765 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java @@ -1,3 +1,42 @@ package frc.robot.subsystems.intake; -public class IntakeIOHardware implements IntakeIO {} +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.SparkMaxConfig; + +public class IntakeIOHardware implements IntakeIO { + SparkMax armMotor = new SparkMax(5, MotorType.kBrushless); + SparkMax wheelMotor = new SparkMax(6, MotorType.kBrushless); + RelativeEncoder armEncoder = armMotor.getEncoder(); + RelativeEncoder wheelEncoder = wheelMotor.getEncoder(); + SparkMaxConfig armConfig; + SparkMaxConfig wheelConfig; + + public IntakeIOHardware() { + armConfig = new SparkMaxConfig(); + wheelConfig = new SparkMaxConfig(); + // armMotor.configure(armConfig, null, null); + // wheelMotor.configure(armConfig, null, null); + } + + @Override + public void setArmSpeed(double speed) { + armMotor.set(speed); + } + + @Override + public void setWheelSpeed(double speed) { + wheelMotor.set(speed); + } + + @Override + public void updateInputs(IntakeIOInputs inputs){ + inputs.armMotorVelocityRPM = armEncoder.getVelocity(); + inputs.wheelMotorVelocityRPM = wheelEncoder.getVelocity(); + inputs.armMotorPositionsRotations = armEncoder.getPosition(); + inputs.wheelMotorPositionRotations = wheelEncoder.getPosition(); + } + +} From 553072a8127c986071ef1af78d636a4ebd934c30 Mon Sep 17 00:00:00 2001 From: Matthew McGrath Date: Thu, 5 Feb 2026 19:39:18 -0500 Subject: [PATCH 16/61] added to and made edits to the intake subsystem --- .../frc/robot/subsystems/intake/Intake.java | 58 ++++++++++++++++++- .../frc/robot/subsystems/intake/IntakeIO.java | 9 ++- .../subsystems/intake/IntakeIOHardware.java | 2 + 3 files changed, 67 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 145f3f1..f407349 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -4,14 +4,70 @@ package frc.robot.subsystems.intake; +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; public class Intake extends SubsystemBase { /** Creates a new Intake. */ - public Intake() {} + + private final IntakeIO io; + private final IntakeIOAutoLogged inputs = new IntakeIOAutoLogged(); + + public Intake(IntakeIO io) { + this.io = io; + } +/** + * Command to run the arm + * @param speed runs the arm at a set speed + * @return runs the arm at a speed on every iteration until end when it stops the running + */ + public Command runArm(double speed) { + return Commands.runEnd( + () -> io.setArmSpeed(speed), + () -> io.setArmSpeed(0.0), + this); + } +/** + * Command to run the feeder + * @param speed runs the feeder at a set speed + * @return runs the feeder at a speed on every iteration until end when it stops the running + */ + public Command runFeeder(double speed) { + return Commands.runEnd( + () -> io.setWheelSpeed(speed), + () -> io.setWheelSpeed(0.0), + this); + } +//potential sequences for commands in future + + + // public Command extendArmSequence() { + // return Commands.run(() -> runArm(.5), this) + // .andThen(Commands.waitUntil()) + // .finallyDo(Commands.runOnce(() -> runArm(0))); + // } + + // public Command retractArmSequence() { + // return Commands.run(() -> runArm(-0.5), this) + // .andThen(Commands.waitUntil()) + // .finallyDo(Commands.runOnce(() -> runArm(0))); + // } + + // public Command runFeederSequence() { + // return Commands.run(() -> runFeeder(.5), this) + // .andThen(Commands.waitUntil()) + // .finallyDo(Commands.runOnce(() -> runArm(0))); + // } @Override public void periodic() { // This method will be called once per scheduler run + io.updateInputs(inputs); + Logger.processInputs("Intake", inputs); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index edbd4de..12387e6 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -24,7 +24,14 @@ public static class IntakeIOInputs { double wheelMotorPositionRotations = 0.0; } - + /** + * method to set the speed of the arm + * @param speed determines the speed of the arm on a scale of -1 to 1 + */ default void setArmSpeed(double speed){} + /** + * method to set the speed of the wheel + * @param speed determines the speed of the wheel on a scale of -1 to 1 + */ default void setWheelSpeed(double speed){} } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java index b2f2765..1c8a3fa 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java @@ -6,6 +6,8 @@ import com.revrobotics.spark.config.EncoderConfig; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.wpilibj.DigitalInput; + public class IntakeIOHardware implements IntakeIO { SparkMax armMotor = new SparkMax(5, MotorType.kBrushless); SparkMax wheelMotor = new SparkMax(6, MotorType.kBrushless); From 1f47b96c299d6970427fa1196b6f79726bbdc892 Mon Sep 17 00:00:00 2001 From: Ryan Hefferon Date: Fri, 6 Feb 2026 18:20:06 -0500 Subject: [PATCH 17/61] add all the io layers of the guts subsystem, including objects, methods, and inputs, and create commands to run the guts forward and backward. --- .../java/frc/robot/subsystems/guts/Guts.java | 30 ++++++++++++- .../frc/robot/subsystems/guts/GutsIO.java | 14 +++++- .../robot/subsystems/guts/GutsIOSparkMax.java | 43 +++++++++++++++++++ .../robot/subsystems/guts/GutsIOTalonFX.java | 3 -- 4 files changed, 84 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java delete mode 100644 src/main/java/frc/robot/subsystems/guts/GutsIOTalonFX.java diff --git a/src/main/java/frc/robot/subsystems/guts/Guts.java b/src/main/java/frc/robot/subsystems/guts/Guts.java index 3809179..eed95e9 100644 --- a/src/main/java/frc/robot/subsystems/guts/Guts.java +++ b/src/main/java/frc/robot/subsystems/guts/Guts.java @@ -4,14 +4,42 @@ package frc.robot.subsystems.guts; +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Guts extends SubsystemBase { + + public final GutsIO io; + public GutsIOInputsAutoLogged inputs = new GutsIOInputsAutoLogged(); + /** Creates a new Guts. */ - public Guts() {} + public Guts(GutsIO io) { + this.io = io; + } + + public Command runGutsForward(){ + return Commands.runEnd( + () -> io.setLeftGutMotorSpeed(0.5), + () -> io.setLeftGutMotorSpeed(0), + this + ); + } + + public Command runGutsBackward(){ + return Commands.runEnd( + () -> io.setLeftGutMotorSpeed(-0.5), + () -> io.setLeftGutMotorSpeed(0), + this + ); + } @Override public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Guts", inputs); // This method will be called once per scheduler run } } diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIO.java b/src/main/java/frc/robot/subsystems/guts/GutsIO.java index 68f158c..ab7b8a2 100644 --- a/src/main/java/frc/robot/subsystems/guts/GutsIO.java +++ b/src/main/java/frc/robot/subsystems/guts/GutsIO.java @@ -3,8 +3,18 @@ import org.littletonrobotics.junction.AutoLog; public interface GutsIO { - default void updateInputs(GutsIOInputs inputs) {} + default void updateInputs(GutsIOInputs inputs) { + } @AutoLog - public static class GutsIOInputs {} + public static class GutsIOInputs { + public double rightGutMotorVelocityRPM = 0.0; + public double rightGutMotorPositionRot = 0.0; + public double leftGutMotorVelocityRPM = 0.0; + public double leftGutMotorPositionRot = 0.0; + } + + default void setLeftGutMotorSpeed(double speed) { + } + } diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java b/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java new file mode 100644 index 0000000..1433edf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java @@ -0,0 +1,43 @@ +package frc.robot.subsystems.guts; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.config.SparkMaxConfig; + +public class GutsIOSparkMax implements GutsIO { + + private final SparkMax leftGutMotor = new SparkMax(0, MotorType.kBrushless); + private final SparkMax rightGutMotor = new SparkMax(1, MotorType.kBrushless); + private final RelativeEncoder leftGutEncoder = leftGutMotor.getEncoder(); + private final RelativeEncoder rightGutEncoder = rightGutMotor.getEncoder(); + private final SparkMaxConfig leftGutMotorConfig; + private final SparkMaxConfig rightGutMotorConfig; + + + public GutsIOSparkMax() { + leftGutMotorConfig = new SparkMaxConfig(); + rightGutMotorConfig = new SparkMaxConfig(); + + rightGutMotorConfig.follow(leftGutMotor, true); + + //fix later to correctly configure motors + //leftGutMotor.configure(leftGutMotorConfig, null, null); + //rightGutMotor.configure(rightGutMotorConfig, null, null); + } + + @Override + public void setLeftGutMotorSpeed(double speed) { + leftGutMotor.set(speed); + } + + @Override + public void updateInputs(GutsIOInputs inputs) { + inputs.leftGutMotorPositionRot = leftGutEncoder.getPosition(); + inputs.leftGutMotorVelocityRPM = leftGutEncoder.getVelocity(); + inputs.rightGutMotorPositionRot = rightGutEncoder.getPosition(); + inputs.rightGutMotorVelocityRPM = rightGutEncoder.getVelocity(); + } + +} diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIOTalonFX.java b/src/main/java/frc/robot/subsystems/guts/GutsIOTalonFX.java deleted file mode 100644 index 433fd18..0000000 --- a/src/main/java/frc/robot/subsystems/guts/GutsIOTalonFX.java +++ /dev/null @@ -1,3 +0,0 @@ -package frc.robot.subsystems.guts; - -public class GutsIOTalonFX implements GutsIO {} From 2402cb294226ceb93b1704e800715964099a0776 Mon Sep 17 00:00:00 2001 From: Ryan Hefferon Date: Fri, 6 Feb 2026 18:31:04 -0500 Subject: [PATCH 18/61] add JavaDocs to all major classes, methods, and commands. --- .../java/frc/robot/subsystems/guts/Guts.java | 27 +++++++++++-------- .../frc/robot/subsystems/guts/GutsIO.java | 9 +++++++ .../robot/subsystems/guts/GutsIOSparkMax.java | 15 +++++++---- 3 files changed, 35 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/guts/Guts.java b/src/main/java/frc/robot/subsystems/guts/Guts.java index eed95e9..917b729 100644 --- a/src/main/java/frc/robot/subsystems/guts/Guts.java +++ b/src/main/java/frc/robot/subsystems/guts/Guts.java @@ -10,6 +10,11 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +/** + * This class updates and stores the values of the inputs periodically, and + * contains commands to run the gut motors forward and backward. + * @author Ryan Hefferon + */ public class Guts extends SubsystemBase { public final GutsIO io; @@ -20,20 +25,20 @@ public Guts(GutsIO io) { this.io = io; } - public Command runGutsForward(){ + /** Runs the gut motors forward at 0.5 speed, then stops them when finished. */ + public Command runGutsForward() { return Commands.runEnd( - () -> io.setLeftGutMotorSpeed(0.5), - () -> io.setLeftGutMotorSpeed(0), - this - ); + () -> io.setLeftGutMotorSpeed(0.5), + () -> io.setLeftGutMotorSpeed(0), + this); } - public Command runGutsBackward(){ + /** Runs the gut motors backward at 0.5 speed, then stops them when finished. */ + public Command runGutsBackward() { return Commands.runEnd( - () -> io.setLeftGutMotorSpeed(-0.5), - () -> io.setLeftGutMotorSpeed(0), - this - ); + () -> io.setLeftGutMotorSpeed(-0.5), + () -> io.setLeftGutMotorSpeed(0), + this); } @Override @@ -42,4 +47,4 @@ public void periodic() { Logger.processInputs("Guts", inputs); // This method will be called once per scheduler run } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIO.java b/src/main/java/frc/robot/subsystems/guts/GutsIO.java index ab7b8a2..a47dd4a 100644 --- a/src/main/java/frc/robot/subsystems/guts/GutsIO.java +++ b/src/main/java/frc/robot/subsystems/guts/GutsIO.java @@ -2,10 +2,18 @@ import org.littletonrobotics.junction.AutoLog; +/** + * This IO interface contains the class which initializes all the inputs as well as + * default methods to update the values of the inputs and set the speed of the motors. + * @author Ryan Hefferon + */ public interface GutsIO { + + /** Updates the values of all the inputs using the physical encoders. */ default void updateInputs(GutsIOInputs inputs) { } + /** Contains all the inputs regarding motors to be stored as data. */ @AutoLog public static class GutsIOInputs { public double rightGutMotorVelocityRPM = 0.0; @@ -14,6 +22,7 @@ public static class GutsIOInputs { public double leftGutMotorPositionRot = 0.0; } + /** Sets the gut motor to a specific speed ranging from -1.0 to 1.0 */ default void setLeftGutMotorSpeed(double speed) { } diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java b/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java index 1433edf..6e6eb88 100644 --- a/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java @@ -3,9 +3,15 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.config.SparkMaxConfig; +/** + * This class contains all of the physical objects: two motors and two + * corresponding encoders. It also implements the default methods specified in + * the IO interface to set the speed of the physical motor and update the input + * values using the encoders. + * @author Ryan Hefferon + */ public class GutsIOSparkMax implements GutsIO { private final SparkMax leftGutMotor = new SparkMax(0, MotorType.kBrushless); @@ -15,16 +21,15 @@ public class GutsIOSparkMax implements GutsIO { private final SparkMaxConfig leftGutMotorConfig; private final SparkMaxConfig rightGutMotorConfig; - public GutsIOSparkMax() { leftGutMotorConfig = new SparkMaxConfig(); rightGutMotorConfig = new SparkMaxConfig(); rightGutMotorConfig.follow(leftGutMotor, true); - //fix later to correctly configure motors - //leftGutMotor.configure(leftGutMotorConfig, null, null); - //rightGutMotor.configure(rightGutMotorConfig, null, null); + // fix later to correctly configure motors + // leftGutMotor.configure(leftGutMotorConfig, null, null); + // rightGutMotor.configure(rightGutMotorConfig, null, null); } @Override From 4d7e75c344c3d59a18dd096154c06b7b492fe42f Mon Sep 17 00:00:00 2001 From: Ryan Hefferon Date: Fri, 6 Feb 2026 19:11:52 -0500 Subject: [PATCH 19/61] fix an error to allow each gut to run separately instead of only being able to run them both. --- .../java/frc/robot/subsystems/guts/Guts.java | 24 +++++++++++++++---- .../frc/robot/subsystems/guts/GutsIO.java | 7 ++++-- .../robot/subsystems/guts/GutsIOSparkMax.java | 14 +++++++---- 3 files changed, 34 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/guts/Guts.java b/src/main/java/frc/robot/subsystems/guts/Guts.java index 917b729..7dc2800 100644 --- a/src/main/java/frc/robot/subsystems/guts/Guts.java +++ b/src/main/java/frc/robot/subsystems/guts/Guts.java @@ -25,22 +25,38 @@ public Guts(GutsIO io) { this.io = io; } - /** Runs the gut motors forward at 0.5 speed, then stops them when finished. */ - public Command runGutsForward() { + /** Runs the left gut motor forward at 0.5 speed, then stops it when finished. */ + public Command runLeftGutForward() { return Commands.runEnd( () -> io.setLeftGutMotorSpeed(0.5), () -> io.setLeftGutMotorSpeed(0), this); } - /** Runs the gut motors backward at 0.5 speed, then stops them when finished. */ - public Command runGutsBackward() { + /** Runs the right gut motor forward at 0.5 speed, then stops it when finished. */ + public Command runRightGutForward() { + return Commands.runEnd( + () -> io.setRightGutMotorSpeed(0.5), + () -> io.setRightGutMotorSpeed(0), + this); + } + + /** Runs the left gut motor backward at 0.5 speed, then stops it when finished. */ + public Command runLeftGutBackward() { return Commands.runEnd( () -> io.setLeftGutMotorSpeed(-0.5), () -> io.setLeftGutMotorSpeed(0), this); } + /** Runs the right gut motor backward at 0.5 speed, then stops it when finished. */ + public Command runRightGutBackward() { + return Commands.runEnd( + () -> io.setRightGutMotorSpeed(-0.5), + () -> io.setRightGutMotorSpeed(0), + this); + } + @Override public void periodic() { io.updateInputs(inputs); diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIO.java b/src/main/java/frc/robot/subsystems/guts/GutsIO.java index a47dd4a..89a296d 100644 --- a/src/main/java/frc/robot/subsystems/guts/GutsIO.java +++ b/src/main/java/frc/robot/subsystems/guts/GutsIO.java @@ -4,7 +4,7 @@ /** * This IO interface contains the class which initializes all the inputs as well as - * default methods to update the values of the inputs and set the speed of the motors. + * default methods to update the values of the inputs and set the speed of each of the motors. * @author Ryan Hefferon */ public interface GutsIO { @@ -22,8 +22,11 @@ public static class GutsIOInputs { public double leftGutMotorPositionRot = 0.0; } - /** Sets the gut motor to a specific speed ranging from -1.0 to 1.0 */ + /** Sets the left gut motor to a specific speed ranging from -1.0 to 1.0 */ default void setLeftGutMotorSpeed(double speed) { } + default void setRightGutMotorSpeed(double speed) { + } + } diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java b/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java index 6e6eb88..fe090d7 100644 --- a/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java @@ -3,6 +3,8 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; +import com.revrobotics.ResetMode; +import com.revrobotics.PersistMode; import com.revrobotics.spark.config.SparkMaxConfig; /** @@ -25,11 +27,8 @@ public GutsIOSparkMax() { leftGutMotorConfig = new SparkMaxConfig(); rightGutMotorConfig = new SparkMaxConfig(); - rightGutMotorConfig.follow(leftGutMotor, true); - - // fix later to correctly configure motors - // leftGutMotor.configure(leftGutMotorConfig, null, null); - // rightGutMotor.configure(rightGutMotorConfig, null, null); + leftGutMotor.configure(leftGutMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + rightGutMotor.configure(rightGutMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); } @Override @@ -37,6 +36,11 @@ public void setLeftGutMotorSpeed(double speed) { leftGutMotor.set(speed); } + @Override + public void setRightGutMotorSpeed(double speed) { + rightGutMotor.set(speed); + } + @Override public void updateInputs(GutsIOInputs inputs) { inputs.leftGutMotorPositionRot = leftGutEncoder.getPosition(); From 6ca934739aa1799a30189787e20bd6d6bf03a953 Mon Sep 17 00:00:00 2001 From: Ryan Hefferon Date: Fri, 6 Feb 2026 19:42:44 -0500 Subject: [PATCH 20/61] alter the gut subsystem to contain an enum and allow us to create a separate instance of it for each of the two guts. --- src/main/java/frc/robot/RobotContainer.java | 65 +++++++++++-------- .../java/frc/robot/subsystems/guts/Guts.java | 62 ++++++++++-------- .../frc/robot/subsystems/guts/GutsIO.java | 17 ++--- .../robot/subsystems/guts/GutsIOSparkMax.java | 35 ++++------ 4 files changed, 91 insertions(+), 88 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f1b07c5..4796bdf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,44 +25,59 @@ import frc.robot.util.AllianceFlipUtil; import frc.robot.util.FieldConstants; import frc.robot.util.FieldConstants.Hub; +import frc.robot.subsystems.guts.Guts; +import frc.robot.subsystems.guts.GutsIO; +import frc.robot.subsystems.guts.Guts.GutSide; public class RobotContainer { private final CommandXboxController driver = new CommandXboxController(0); private final Drive drive; private final Vision vision; + private final Guts leftGut; + private final Guts rightGut; public RobotContainer() { switch (Constants.kCurrentMode) { case REAL: - drive = - new Drive( - new GyroIOPigeon2(), - new ModuleIOTalonFX(ModuleConstants.FrontLeft), - new ModuleIOTalonFX(ModuleConstants.FrontRight), - new ModuleIOTalonFX(ModuleConstants.BackLeft), - new ModuleIOTalonFX(ModuleConstants.BackRight)); + drive = new Drive( + new GyroIOPigeon2(), + new ModuleIOTalonFX(ModuleConstants.FrontLeft), + new ModuleIOTalonFX(ModuleConstants.FrontRight), + new ModuleIOTalonFX(ModuleConstants.BackLeft), + new ModuleIOTalonFX(ModuleConstants.BackRight)); vision = new Vision(null, null); break; case SIM: - drive = - new Drive( - new GyroIO() {}, - new ModuleIOSim(ModuleConstants.FrontLeft), - new ModuleIOSim(ModuleConstants.FrontRight), - new ModuleIOSim(ModuleConstants.BackLeft), - new ModuleIOSim(ModuleConstants.BackRight)); + drive = new Drive( + new GyroIO() { + }, + new ModuleIOSim(ModuleConstants.FrontLeft), + new ModuleIOSim(ModuleConstants.FrontRight), + new ModuleIOSim(ModuleConstants.BackLeft), + new ModuleIOSim(ModuleConstants.BackRight)); vision = new Vision(null, null); + leftGut = new Guts(GutSide.LEFT, new GutsIO() { + + }); + rightGut = new Guts(GutSide.RIGHT, new GutsIO() { + + }); + break; case REPLAY: default: - drive = - new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}); + drive = new Drive( + new GyroIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }); vision = new Vision(null, new CameraIO[] {}); break; } @@ -84,8 +99,7 @@ private void configureBindings() { () -> -driver.getLeftX(), // ySupplier () -> { Pose2d robotPose = RobotState.getInstance().getEstimatedPose(); - Translation2d target = - AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d()); + Translation2d target = AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d()); Translation2d delta = target.minus(robotPose.getTranslation()); @@ -103,9 +117,8 @@ private void configureBindings() { } public void robotPeriodic() { - OdometryObservation obs = - new OdometryObservation( - Timer.getTimestamp(), drive.getModulePositions(), drive.getRawGyroRotation()); + OdometryObservation obs = new OdometryObservation( + Timer.getTimestamp(), drive.getModulePositions(), drive.getRawGyroRotation()); RobotState.getInstance().addOdometryObservation(obs); } diff --git a/src/main/java/frc/robot/subsystems/guts/Guts.java b/src/main/java/frc/robot/subsystems/guts/Guts.java index 7dc2800..b5a8d96 100644 --- a/src/main/java/frc/robot/subsystems/guts/Guts.java +++ b/src/main/java/frc/robot/subsystems/guts/Guts.java @@ -12,55 +12,61 @@ /** * This class updates and stores the values of the inputs periodically, and - * contains commands to run the gut motors forward and backward. + * contains commands to run the gut motor forward and backward. + * * @author Ryan Hefferon */ public class Guts extends SubsystemBase { + private final GutSide side; public final GutsIO io; public GutsIOInputsAutoLogged inputs = new GutsIOInputsAutoLogged(); /** Creates a new Guts. */ - public Guts(GutsIO io) { + public Guts(GutSide side, GutsIO io) { this.io = io; + this.side = side; } - /** Runs the left gut motor forward at 0.5 speed, then stops it when finished. */ - public Command runLeftGutForward() { + /** + * Runs the gut motor forward at 0.5 speed, then stops it when finished. + */ + public Command runGutForward() { return Commands.runEnd( - () -> io.setLeftGutMotorSpeed(0.5), - () -> io.setLeftGutMotorSpeed(0), + () -> io.setGutMotorSpeed(0.5), + () -> io.setGutMotorSpeed(0), this); } - /** Runs the right gut motor forward at 0.5 speed, then stops it when finished. */ - public Command runRightGutForward() { + /** + * Runs the gut motor backward at 0.5 speed, then stops it when finished. + */ + public Command runGutBackward() { return Commands.runEnd( - () -> io.setRightGutMotorSpeed(0.5), - () -> io.setRightGutMotorSpeed(0), - this); - } - - /** Runs the left gut motor backward at 0.5 speed, then stops it when finished. */ - public Command runLeftGutBackward() { - return Commands.runEnd( - () -> io.setLeftGutMotorSpeed(-0.5), - () -> io.setLeftGutMotorSpeed(0), - this); - } - - /** Runs the right gut motor backward at 0.5 speed, then stops it when finished. */ - public Command runRightGutBackward() { - return Commands.runEnd( - () -> io.setRightGutMotorSpeed(-0.5), - () -> io.setRightGutMotorSpeed(0), + () -> io.setGutMotorSpeed(-0.5), + () -> io.setGutMotorSpeed(0), this); } @Override public void periodic() { io.updateInputs(inputs); - Logger.processInputs("Guts", inputs); + Logger.processInputs("Guts/" + side.getName(), inputs); // This method will be called once per scheduler run } -} \ No newline at end of file + + public enum GutSide { + LEFT("Left"), + RIGHT("Right"); + + private final String name; + + private GutSide(String name) { + this.name = name; + } + + public String getName() { + return name; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIO.java b/src/main/java/frc/robot/subsystems/guts/GutsIO.java index 89a296d..c7aaca4 100644 --- a/src/main/java/frc/robot/subsystems/guts/GutsIO.java +++ b/src/main/java/frc/robot/subsystems/guts/GutsIO.java @@ -3,8 +3,8 @@ import org.littletonrobotics.junction.AutoLog; /** - * This IO interface contains the class which initializes all the inputs as well as - * default methods to update the values of the inputs and set the speed of each of the motors. + * This IO interface contains the class which initializes all the inputs as well + * as default methods to update the values of the inputs and set the speed of the motor. * @author Ryan Hefferon */ public interface GutsIO { @@ -16,17 +16,12 @@ default void updateInputs(GutsIOInputs inputs) { /** Contains all the inputs regarding motors to be stored as data. */ @AutoLog public static class GutsIOInputs { - public double rightGutMotorVelocityRPM = 0.0; - public double rightGutMotorPositionRot = 0.0; - public double leftGutMotorVelocityRPM = 0.0; - public double leftGutMotorPositionRot = 0.0; + public double GutMotorVelocityRPM = 0.0; + public double GutMotorPositionRot = 0.0; } - /** Sets the left gut motor to a specific speed ranging from -1.0 to 1.0 */ - default void setLeftGutMotorSpeed(double speed) { - } - - default void setRightGutMotorSpeed(double speed) { + /** Sets the gut motor to a specific speed ranging from -1.0 to 1.0 */ + default void setGutMotorSpeed(double speed) { } } diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java b/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java index fe090d7..31c7940 100644 --- a/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java @@ -8,45 +8,34 @@ import com.revrobotics.spark.config.SparkMaxConfig; /** - * This class contains all of the physical objects: two motors and two - * corresponding encoders. It also implements the default methods specified in + * This class contains all of the physical objects: one motor and its + * corresponding encoder. It also implements the default methods specified in * the IO interface to set the speed of the physical motor and update the input * values using the encoders. + * * @author Ryan Hefferon */ public class GutsIOSparkMax implements GutsIO { - private final SparkMax leftGutMotor = new SparkMax(0, MotorType.kBrushless); - private final SparkMax rightGutMotor = new SparkMax(1, MotorType.kBrushless); - private final RelativeEncoder leftGutEncoder = leftGutMotor.getEncoder(); - private final RelativeEncoder rightGutEncoder = rightGutMotor.getEncoder(); - private final SparkMaxConfig leftGutMotorConfig; - private final SparkMaxConfig rightGutMotorConfig; + private final SparkMax GutMotor = new SparkMax(0, MotorType.kBrushless); + private final RelativeEncoder GutEncoder = GutMotor.getEncoder(); + private final SparkMaxConfig GutMotorConfig; public GutsIOSparkMax() { - leftGutMotorConfig = new SparkMaxConfig(); - rightGutMotorConfig = new SparkMaxConfig(); + GutMotorConfig = new SparkMaxConfig(); - leftGutMotor.configure(leftGutMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - rightGutMotor.configure(rightGutMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + GutMotor.configure(GutMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); } @Override - public void setLeftGutMotorSpeed(double speed) { - leftGutMotor.set(speed); - } - - @Override - public void setRightGutMotorSpeed(double speed) { - rightGutMotor.set(speed); + public void setGutMotorSpeed(double speed) { + GutMotor.set(speed); } @Override public void updateInputs(GutsIOInputs inputs) { - inputs.leftGutMotorPositionRot = leftGutEncoder.getPosition(); - inputs.leftGutMotorVelocityRPM = leftGutEncoder.getVelocity(); - inputs.rightGutMotorPositionRot = rightGutEncoder.getPosition(); - inputs.rightGutMotorVelocityRPM = rightGutEncoder.getVelocity(); + inputs.GutMotorPositionRot = GutEncoder.getPosition(); + inputs.GutMotorVelocityRPM = GutEncoder.getVelocity(); } } From 9d7b234840018008699fb97d196507273e2f458a Mon Sep 17 00:00:00 2001 From: Maxwell Morgan Date: Mon, 9 Feb 2026 17:23:36 -0500 Subject: [PATCH 21/61] Add shooter, hood, turret, and flywheel classes --- src/main/java/frc/robot/Constants.java | 848 ++++++++---------- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 26 +- src/main/java/frc/robot/RobotVisualizer.java | 8 +- .../robot/subsystems/drive/ModuleIOSim.java | 4 +- .../frc/robot/subsystems/shooter/Shooter.java | 18 +- .../subsystems/shooter/flywheel/Flywheel.java | 61 ++ .../shooter/flywheel/FlywheelIO.java | 24 + .../shooter/flywheel/FlywheelIOSim.java | 49 + .../shooter/flywheel/FlywheelIOTalonFX.java | 62 ++ .../robot/subsystems/shooter/hood/Hood.java | 32 +- .../robot/subsystems/shooter/hood/HoodIO.java | 15 +- .../subsystems/shooter/hood/HoodIOSim.java | 41 +- .../shooter/hood/HoodIOSparkMax.java | 11 +- .../subsystems/shooter/turret/Turret.java | 23 +- .../subsystems/shooter/turret/TurretIO.java | 10 +- .../shooter/turret/TurretIOSim.java | 15 +- .../shooter/turret/TurretIOSparkMax.java | 2 +- 18 files changed, 713 insertions(+), 538 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9b6b5bd..a7374ef 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -18,13 +18,13 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; -import com.ctre.phoenix6.swerve.SwerveDrivetrain; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; @@ -36,14 +36,12 @@ import com.pathplanner.lib.config.RobotConfig; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Distance; @@ -55,492 +53,388 @@ import frc.robot.util.GeomUtil; /** - * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running - * on a roboRIO. Change the value of "simMode" to switch between "sim" (physics sim) and "replay" + * This class defines the runtime mode used by AdvantageKit. The mode is always + * "real" when running + * on a roboRIO. Change the value of "simMode" to switch between "sim" (physics + * sim) and "replay" * (log replay from a file). */ public final class Constants { - public static final double kLoopPeriodSeconds = 0.02; - - public static final Mode kSimMode = Mode.SIM; - public static final Mode kCurrentMode = RobotBase.isReal() ? Mode.REAL : kSimMode; - - public static enum Mode { - /** Running on a real robot. */ - REAL, - - /** Running a physics simulator. */ - SIM, - - /** Replaying from a log file. */ - REPLAY - } - - public static final int kDriverControllerPort = 0; - public static final int kAuxControllerPort = 1; - - public static boolean kDisableHAL = false; - - public static void disableHAL() { - kDisableHAL = true; - } - - public static final class DriveConstants { - public static final SwerveDriveKinematics kSwerveKinematics = - new SwerveDriveKinematics(Drive.getModuleTranslations()); - - public static final double kOdometryFrequency = - ModuleConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0; - public static final double kDriveBaseRadius = - Math.max( - Math.max( - Math.hypot( - ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), - Math.hypot( - ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY)), - Math.max( - Math.hypot(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), - Math.hypot( - ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY))); - - public static final Translation2d[] kModuleTranslations = - new Translation2d[] { - new Translation2d( - ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), - new Translation2d( - ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY), - new Translation2d(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), - new Translation2d( - ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY) - }; + public static final double kLoopPeriodSeconds = 0.02; - // TODO: Update for robot - // PathPlanner config constants - public static final double kRobotMassKG = 74.088; - public static final double kRobotMOI = 6.883; - /** Coefficient of friction */ - public static final double kWheelCOF = 1.2; - - public static final RobotConfig kPathplannerConfig = - new RobotConfig( - kRobotMOI, - kRobotMOI, - new ModuleConfig( - ModuleConstants.FrontLeft.WheelRadius, - ModuleConstants.kSpeedAt12Volts.in(MetersPerSecond), - kWheelCOF, - DCMotor.getKrakenX60Foc(1) - .withReduction(ModuleConstants.FrontLeft.DriveMotorGearRatio), - ModuleConstants.FrontLeft.SlipCurrent, - 1), - kModuleTranslations); - - public static final class ModuleConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with - // the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - // TODO: Update for robot - private static final Slot0Configs steerGains = - new Slot0Configs() - .withKP(100) - .withKI(0) - .withKD(0.5) - .withKS(0.1) - .withKV(1.91) - .withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - // TODO: Update for robot - private static final Slot0Configs driveGains = - new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = - ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = - ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = - DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = - SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to - // RemoteCANcoder - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - // TODO: Update for robot - private static final Current kSlipCurrent = Amps.of(120.0); - - // Initial configs for the drive and steer motors and the azimuth encoder; these - // cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API - // documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = - new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a - // relatively - // low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true)); - private static final CANcoderConfiguration encoderInitialConfigs = - new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; - - // CAN bus that the devices are located on; - // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot"); - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - // TODO: Update for robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - // TODO: Update for robot - private static final double kCoupleRatio = 3.8181818181818183; - // TODO: Update for robot - private static final double kDriveGearRatio = 7.363636363636365; - private static final double kSteerGearRatio = 15.42857142857143; - private static final Distance kWheelRadius = Inches.of(2.167); - // TODO: Update for robot - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - // TODO: Update for robot - private static final int kPigeonId = 1; - - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - - public static final SwerveDrivetrainConstants DrivetrainConstants = - new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - ConstantCreator = - new SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - - // TODO: Update for robot - // Front Left - private static final int kFrontLeftDriveMotorId = 3; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 1; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); - private static final boolean kFrontLeftSteerMotorInverted = true; - private static final boolean kFrontLeftEncoderInverted = false; - - private static final Distance kFrontLeftXPos = Inches.of(10); - private static final Distance kFrontLeftYPos = Inches.of(10); - // TODO: Update for robot - // Front Right - private static final int kFrontRightDriveMotorId = 1; - private static final int kFrontRightSteerMotorId = 0; - private static final int kFrontRightEncoderId = 0; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); - private static final boolean kFrontRightSteerMotorInverted = true; - private static final boolean kFrontRightEncoderInverted = false; - - private static final Distance kFrontRightXPos = Inches.of(10); - private static final Distance kFrontRightYPos = Inches.of(-10); - // TODO: Update for robot - // Back Left - private static final int kBackLeftDriveMotorId = 7; - private static final int kBackLeftSteerMotorId = 6; - private static final int kBackLeftEncoderId = 3; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); - private static final boolean kBackLeftSteerMotorInverted = true; - private static final boolean kBackLeftEncoderInverted = false; - - private static final Distance kBackLeftXPos = Inches.of(-10); - private static final Distance kBackLeftYPos = Inches.of(10); - // TODO: Update for robot - // Back Right - private static final int kBackRightDriveMotorId = 5; - private static final int kBackRightSteerMotorId = 4; - private static final int kBackRightEncoderId = 2; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); - private static final boolean kBackRightSteerMotorInverted = true; - private static final boolean kBackRightEncoderInverted = false; - - private static final Distance kBackRightXPos = Inches.of(-10); - private static final Distance kBackRightYPos = Inches.of(-10); - - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, - kFrontLeftDriveMotorId, - kFrontLeftEncoderId, - kFrontLeftEncoderOffset, - kFrontLeftXPos, - kFrontLeftYPos, - kInvertLeftSide, - kFrontLeftSteerMotorInverted, - kFrontLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, - kFrontRightDriveMotorId, - kFrontRightEncoderId, - kFrontRightEncoderOffset, - kFrontRightXPos, - kFrontRightYPos, - kInvertRightSide, - kFrontRightSteerMotorInverted, - kFrontRightEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, - kBackLeftDriveMotorId, - kBackLeftEncoderId, - kBackLeftEncoderOffset, - kBackLeftXPos, - kBackLeftYPos, - kInvertLeftSide, - kBackLeftSteerMotorInverted, - kBackLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, - kBackRightDriveMotorId, - kBackRightEncoderId, - kBackRightEncoderOffset, - kBackRightXPos, - kBackRightYPos, - kInvertRightSide, - kBackRightSteerMotorInverted, - kBackRightEncoderInverted); - - /** - * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot - * program,. - */ - // public static CommandSwerveDrivetrain createDrivetrain() { - // return new CommandSwerveDrivetrain( - // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); - // } - - /** - * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. - */ - public static class TunerSwerveDrivetrain - extends SwerveDrivetrain { - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the - * devices themselves. If they need the devices, they can access them through getters in the - * classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules) { - super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules); - } + public static final Mode kSimMode = Mode.SIM; + public static final Mode kCurrentMode = RobotBase.isReal() ? Mode.REAL : kSimMode; - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the - * devices themselves. If they need the devices, they can access them through getters in the - * classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or - * set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules) { - super( - TalonFX::new, - TalonFX::new, - CANcoder::new, - drivetrainConstants, - odometryUpdateFrequency, - modules); - } + public static enum Mode { + /** Running on a real robot. */ + REAL, + + /** Running a physics simulator. */ + SIM, + + /** Replaying from a log file. */ + REPLAY + } - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the - * devices themselves. If they need the devices, they can access them through getters in the - * classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or - * set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation in the - * form [x, y, theta]áµ€, with units in meters and radians - * @param visionStandardDeviation The standard deviation for vision calculation in the form - * [x, y, theta]áµ€, with units in meters and radians - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules) { - super( - TalonFX::new, - TalonFX::new, - CANcoder::new, - drivetrainConstants, - odometryUpdateFrequency, - odometryStandardDeviation, - visionStandardDeviation, - modules); + public static final int kDriverControllerPort = 0; + public static final int kAuxControllerPort = 1; + + public static boolean kDisableHAL = false; + + public static void disableHAL() { + kDisableHAL = true; + } + + public static final class DriveConstants { + public static final SwerveDriveKinematics kSwerveKinematics = new SwerveDriveKinematics( + Drive.getModuleTranslations()); + + public static final double kOdometryFrequency = ModuleConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0; + public static final double kDriveBaseRadius = Math.max( + Math.max( + Math.hypot( + ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), + Math.hypot( + ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY)), + Math.max( + Math.hypot(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), + Math.hypot( + ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY))); + + public static final Translation2d[] kModuleTranslations = new Translation2d[] { + new Translation2d( + ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), + new Translation2d( + ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY), + new Translation2d(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), + new Translation2d( + ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY) + }; + + // TODO: Update for robot + // PathPlanner config constants + public static final double kRobotMassKG = 74.088; + public static final double kRobotMOI = 6.883; + /** Coefficient of friction */ + public static final double kWheelCOF = 1.2; + + public static final RobotConfig kPathplannerConfig = new RobotConfig( + kRobotMOI, + kRobotMOI, + new ModuleConfig( + ModuleConstants.FrontLeft.WheelRadius, + ModuleConstants.kSpeedAt12Volts.in(MetersPerSecond), + kWheelCOF, + DCMotor.getKrakenX60(1) + .withReduction(ModuleConstants.FrontLeft.DriveMotorGearRatio), + ModuleConstants.FrontLeft.SlipCurrent, + 1), + kModuleTranslations); + + public static final class ModuleConstants { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with + // the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + // TODO: Update for robot + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100) + .withKI(0) + .withKD(0.5) + .withKS(0.1) + .withKV(1.91) + .withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + // TODO: Update for robot + private static final Slot0Configs driveGains = new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0) + .withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to + // RemoteCANcoder + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + // TODO: Update for robot + private static final Current kSlipCurrent = Amps.of(120.0); + + // Initial configs for the drive and steer motors and the azimuth encoder; these + // cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API + // documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a + // relatively + // low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true)); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + // TODO: Update for robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + // TODO: Update for robot + private static final double kCoupleRatio = 3.8181818181818183; + // TODO: Update for robot + private static final double kDriveGearRatio = 7.363636363636365; + private static final double kSteerGearRatio = 15.42857142857143; + private static final Distance kWheelRadius = Inches.of(2.167); + // TODO: Update for robot + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + // TODO: Update for robot + private static final int kPigeonId = 1; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + // TODO: Update for robot + // Front Left + private static final int kFrontLeftDriveMotorId = 3; + private static final int kFrontLeftSteerMotorId = 2; + private static final int kFrontLeftEncoderId = 1; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(10); + private static final Distance kFrontLeftYPos = Inches.of(10); + // TODO: Update for robot + // Front Right + private static final int kFrontRightDriveMotorId = 1; + private static final int kFrontRightSteerMotorId = 0; + private static final int kFrontRightEncoderId = 0; + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(10); + private static final Distance kFrontRightYPos = Inches.of(-10); + // TODO: Update for robot + // Back Left + private static final int kBackLeftDriveMotorId = 7; + private static final int kBackLeftSteerMotorId = 6; + private static final int kBackLeftEncoderId = 3; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); + private static final boolean kBackLeftSteerMotorInverted = true; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-10); + private static final Distance kBackLeftYPos = Inches.of(10); + // TODO: Update for robot + // Back Right + private static final int kBackRightDriveMotorId = 5; + private static final int kBackRightSteerMotorId = 4; + private static final int kBackRightEncoderId = 2; + private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-10); + private static final Distance kBackRightYPos = Inches.of(-10); + + public static final SwerveModuleConstants FrontLeft = ConstantCreator + .createModuleConstants( + kFrontLeftSteerMotorId, + kFrontLeftDriveMotorId, + kFrontLeftEncoderId, + kFrontLeftEncoderOffset, + kFrontLeftXPos, + kFrontLeftYPos, + kInvertLeftSide, + kFrontLeftSteerMotorInverted, + kFrontLeftEncoderInverted); + public static final SwerveModuleConstants FrontRight = ConstantCreator + .createModuleConstants( + kFrontRightSteerMotorId, + kFrontRightDriveMotorId, + kFrontRightEncoderId, + kFrontRightEncoderOffset, + kFrontRightXPos, + kFrontRightYPos, + kInvertRightSide, + kFrontRightSteerMotorInverted, + kFrontRightEncoderInverted); + public static final SwerveModuleConstants BackLeft = ConstantCreator + .createModuleConstants( + kBackLeftSteerMotorId, + kBackLeftDriveMotorId, + kBackLeftEncoderId, + kBackLeftEncoderOffset, + kBackLeftXPos, + kBackLeftYPos, + kInvertLeftSide, + kBackLeftSteerMotorInverted, + kBackLeftEncoderInverted); + public static final SwerveModuleConstants BackRight = ConstantCreator + .createModuleConstants( + kBackRightSteerMotorId, + kBackRightDriveMotorId, + kBackRightEncoderId, + kBackRightEncoderOffset, + kBackRightXPos, + kBackRightYPos, + kInvertRightSide, + kBackRightSteerMotorInverted, + kBackRightEncoderInverted); } - } } - } - - public static final class VisionConstants { - // AprilTag layout - public static AprilTagFieldLayout aprilTagLayout = - AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); - - // Camera names, must match names configured on coprocessor - public static String camera0Name = "camera_0"; - public static String camera1Name = "camera_1"; - - // Robot to camera transforms - // (Not used by Limelight, configure in web UI instead) - public static Transform3d robotToCamera0 = - new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); - public static Transform3d robotToCamera1 = - new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); - - // Basic filtering thresholds - public static double maxAmbiguity = 0.3; - public static double maxZError = 0.75; - - // Standard deviation baselines, for 1 meter distance and 1 tag - // (Adjusted automatically based on distance and # of tags) - public static double linearStdDevBaseline = 0.02; // Meters - public static double angularStdDevBaseline = 0.06; // Radians - - // Standard deviation multipliers for each camera - // (Adjust to trust some cameras more than others) - public static double[] cameraStdDevFactors = - new double[] { - 1.0, // Camera 0 - 1.0 // Camera 1 + + public static final class VisionConstants { + // AprilTag layout + public static AprilTagFieldLayout aprilTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + + // Camera names, must match names configured on coprocessor + public static String camera0Name = "camera_0"; + public static String camera1Name = "camera_1"; + + // Robot to camera transforms + // (Not used by Limelight, configure in web UI instead) + public static Transform3d robotToCamera0 = new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); + public static Transform3d robotToCamera1 = new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); + + // Basic filtering thresholds + public static double maxAmbiguity = 0.3; + public static double maxZError = 0.75; + + // Standard deviation baselines, for 1 meter distance and 1 tag + // (Adjusted automatically based on distance and # of tags) + public static double linearStdDevBaseline = 0.02; // Meters + public static double angularStdDevBaseline = 0.06; // Radians + + // Standard deviation multipliers for each camera + // (Adjust to trust some cameras more than others) + public static double[] cameraStdDevFactors = new double[] { + 1.0, // Camera 0 + 1.0 // Camera 1 }; - // Multipliers to apply for MegaTag 2 observations - public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve - public static double angularStdDevMegatag2Factor = - Double.POSITIVE_INFINITY; // No rotation data available - } + // Multipliers to apply for MegaTag 2 observations + public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve + public static double angularStdDevMegatag2Factor = Double.POSITIVE_INFINITY; // No rotation data available + } - public static final class ShooterConstants { + public static final class ShooterConstants { - public static final class TurretConstants { - public static final double kGearRatio = 10 / 1; - public static final double kMinTurretAngleRad = -3.0 * Math.PI / 2.0; // -270 degrees - public static final double kMaxTurretAngleRad = 3.0 * Math.PI / 2.0; // +270 degrees + public static final class TurretConstants { + public static final double kGearRatio = 10 / 1; + public static final double kMinTurretAngleRad = -3.0 * Math.PI / 2.0; // -270 degrees + public static final double kMaxTurretAngleRad = 3.0 * Math.PI / 2.0; // +270 degrees - public static final double kLeftMotorId = 12; - public static final double kRightMotorId = 13; + public static final double kLeftMotorId = 12; + public static final double kRightMotorId = 13; - // +X = Forward, +Y = Left - public static final Transform3d kRobotToLeftTurret = - new Transform3d(Inches.of(3.749), Inches.of(8.186), Inches.of(13.401), Rotation3d.kZero); + // +X = Forward, +Y = Left + public static final Transform3d kRobotToLeftTurret = new Transform3d(Inches.of(3.749), Inches.of(8.186), + Inches.of(13.401), Rotation3d.kZero); - public static final Transform3d kRobotToRightTurret = - new Transform3d(Inches.of(3.749), Inches.of(-8.314), Inches.of(13.401), Rotation3d.kZero); - } + public static final Transform3d kRobotToRightTurret = new Transform3d(Inches.of(3.749), Inches.of(-8.314), + Inches.of(13.401), Rotation3d.kZero); + } - public static final class HoodConstants { - public static final double kTurretToHoodInches = 1.878; - public static final double kGearRatio = 100 / 1; - - public static final Transform3d kRobotToLeftHood = - new Transform3d( - Inches.of(7.268715), Meters.of(0.20792316), Inches.of(16.018516), Rotation3d.kZero); - - public static final Transform3d kRobotToRightHood = - new Transform3d( - Inches.of(-7.270121), - Inches.of(-(12.062888 - (7.5 / 2.0))), - Inches.of(16.018516), - Rotation3d.kZero); - - public static final Transform3d kLeftTurretToLeftHood = - GeomUtil.toPose3d(HoodConstants.kRobotToLeftHood) - .minus( - GeomUtil.toPose3d(TurretConstants.kRobotToLeftTurret) - .plus( - new Transform3d( - Inches.of(7.268715), Inches.of(0), Inches.of(0), new Rotation3d()))); - - public static final Transform3d kRightTurretToRightHood = - GeomUtil.toPose3d(HoodConstants.kRobotToRightHood) - .minus( - GeomUtil.toPose3d(TurretConstants.kRobotToRightTurret) - .plus( - new Transform3d( - Inches.of(-7.270121), Inches.of(0), Inches.of(0), new Rotation3d()))); + public static final class HoodConstants { + public static final double kTurretToHoodInches = 1.878; + public static final double kGearRatio = 100 / 1; + + public static final Transform3d kRobotToLeftHood = new Transform3d( + Inches.of(7.268715), Meters.of(0.20792316), Inches.of(16.018516), Rotation3d.kZero); + + public static final Transform3d kRobotToRightHood = new Transform3d( + Inches.of(-7.270121), + Inches.of(-(12.062888 - (7.5 / 2.0))), + Inches.of(16.018516), + Rotation3d.kZero); + + public static final Transform3d kLeftTurretToLeftHood = GeomUtil.toPose3d(HoodConstants.kRobotToLeftHood) + .minus( + GeomUtil.toPose3d(TurretConstants.kRobotToLeftTurret) + .plus( + new Transform3d( + Inches.of(7.268715), Inches.of(0), Inches.of(0), + new Rotation3d()))); + + public static final Transform3d kRightTurretToRightHood = GeomUtil.toPose3d(HoodConstants.kRobotToRightHood) + .minus( + GeomUtil.toPose3d(TurretConstants.kRobotToRightTurret) + .plus( + new Transform3d( + Inches.of(-7.270121), Inches.of(0), Inches.of(0), + new Rotation3d()))); + + public static final double kMinAngleRad = Units.degreesToRadians(0); + public static final double kMaxAngleRad = Units.degreesToRadians(40); + } + + public static final class FlywheelConstants { + public static final double kGearRatio = 300; + public static final double kSpeedTolerance = 25.0; + + public static final int kLeftFlywheelID = 2; + + public static final Slot0Configs kGains = new Slot0Configs().withKP(1).withKD(0).withKS(0); + public static final MotorOutputConfigs kOutputConfigs = new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast).withInverted(InvertedValue.Clockwise_Positive); + } } - } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ded80a4..1fb2f38 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -47,7 +47,7 @@ public Robot() { switch (Constants.kCurrentMode) { case REAL: // Running on a real robot, log to a USB stick ("/U/logs") - Logger.addDataReceiver(new WPILOGWriter()); + // Logger.addDataReceiver(new WPILOGWriter()); Logger.addDataReceiver(new NT4Publisher()); break; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6ae50fb..323cba9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.DriveConstants.ModuleConstants; import frc.robot.RobotState.OdometryObservation; import frc.robot.commands.DriveCommands; @@ -20,20 +20,22 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; +import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.Shooter.ShooterSide; -import frc.robot.subsystems.shooter.turret.Turret; +import frc.robot.subsystems.shooter.flywheel.FlywheelIOSim; +import frc.robot.subsystems.shooter.hood.HoodIOSim; import frc.robot.subsystems.shooter.turret.TurretIOSim; import frc.robot.util.AllianceFlipUtil; import frc.robot.util.FieldConstants; import frc.robot.util.FieldConstants.Hub; public class RobotContainer { - private final CommandPS5Controller driver = - new CommandPS5Controller(Constants.kDriverControllerPort); + private final CommandXboxController driver = + new CommandXboxController(Constants.kDriverControllerPort); private Drive drive; - private Turret leftShooter; - private Turret rightShooter; + private Shooter leftShooter; + private Shooter rightShooter; // private Vision vision; public RobotContainer() { @@ -56,8 +58,10 @@ public RobotContainer() { new ModuleIOSim(ModuleConstants.FrontRight), new ModuleIOSim(ModuleConstants.BackLeft), new ModuleIOSim(ModuleConstants.BackRight)); - leftShooter = new Turret(ShooterSide.LEFT, new TurretIOSim()); - rightShooter = new Turret(ShooterSide.RIGHT, new TurretIOSim()); + leftShooter = + new Shooter(ShooterSide.LEFT, new TurretIOSim(), new HoodIOSim(), new FlywheelIOSim()); + rightShooter = + new Shooter(ShooterSide.RIGHT, new TurretIOSim(), new HoodIOSim(), new FlywheelIOSim()); // vision = new Vision(null, null); break; case REPLAY: @@ -90,7 +94,7 @@ private void configureBindings() { () -> AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d()))); driver - .R1() + .rightBumper() .whileTrue( DriveCommands.joystickDriveAtAngle( drive, @@ -107,7 +111,7 @@ private void configureBindings() { })); driver - .triangle() + .y() .onTrue( DriveCommands.turnToPoint( drive, @@ -125,4 +129,6 @@ public void robotPeriodic() { public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } + + public void configureSubsystems() {} } diff --git a/src/main/java/frc/robot/RobotVisualizer.java b/src/main/java/frc/robot/RobotVisualizer.java index 313df35..69b639f 100644 --- a/src/main/java/frc/robot/RobotVisualizer.java +++ b/src/main/java/frc/robot/RobotVisualizer.java @@ -49,13 +49,13 @@ public void log(String key) { leftTurretPose.transformBy( new Transform3d( HoodConstants.kLeftTurretToLeftHood.getTranslation(), - new Rotation3d(0.0, Math.abs(Math.sin(Timer.getFPGATimestamp())) * -0.5, 0.0))); + new Rotation3d(0.0, hoodAngles[0], 0.0))); Pose3d rightHoodPose = rightTurretPose.transformBy( new Transform3d( HoodConstants.kRightTurretToRightHood.getTranslation(), - new Rotation3d(0.0, Math.abs(Math.sin(Timer.getFPGATimestamp())) * -0.5, 0.0))); + new Rotation3d(0.0, hoodAngles[1], 0.0))); Logger.recordOutput( key + "/Components", leftTurretPose, rightTurretPose, leftHoodPose, rightHoodPose); @@ -120,7 +120,7 @@ public double getRightHoodAngle() { * * @param angle A Rotation2d object to be inserted in the angles array. */ - public void setLeftTurretAngle(double angle) { + public void setLeftHoodAngle(double angle) { hoodAngles[0] = angle; } @@ -129,7 +129,7 @@ public void setLeftTurretAngle(double angle) { * * @param angle A double to be inserted in the angles array. */ - public void setLeftHoodAngle(double angle) { + public void setRightHoodAngle(double angle) { hoodAngles[1] = angle; } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index ac73d2b..dc06a39 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -34,8 +34,8 @@ public class ModuleIOSim implements ModuleIO { private static final double DRIVE_KV = 1.0 / Units.rotationsToRadians(1.0 / DRIVE_KV_ROT); private static final double TURN_KP = 8.0; private static final double TURN_KD = 0.0; - private static final DCMotor DRIVE_GEARBOX = DCMotor.getKrakenX60Foc(1); - private static final DCMotor TURN_GEARBOX = DCMotor.getKrakenX60Foc(1); + private static final DCMotor DRIVE_GEARBOX = DCMotor.getKrakenX60(1); + private static final DCMotor TURN_GEARBOX = DCMotor.getKrakenX44(1); private final DCMotorSim driveSim; private final DCMotorSim turnSim; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 68cb95a..fe2dc6e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -7,7 +7,10 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.shooter.flywheel.Flywheel; +import frc.robot.subsystems.shooter.flywheel.FlywheelIO; import frc.robot.subsystems.shooter.hood.Hood; import frc.robot.subsystems.shooter.hood.HoodIO; import frc.robot.subsystems.shooter.turret.Turret; @@ -19,23 +22,34 @@ public class Shooter extends SubsystemBase { private final Turret turret; private final Hood hood; + private final Flywheel flywheel; /** Creates a new Shooter. */ - public Shooter(ShooterSide side, TurretIO turretIO, HoodIO hoodIO) { + public Shooter(ShooterSide side, TurretIO turretIO, HoodIO hoodIO, FlywheelIO flywheelIO) { this.side = side; this.turret = new Turret(side, turretIO); this.hood = new Hood(side, hoodIO); + this.flywheel = new Flywheel(side, flywheelIO); } @Override public void periodic() { turret.periodic(); hood.periodic(); + flywheel.periodic(); } public Command trackTarget( Supplier robotPoseSupplier, Supplier targetSupplier) { - return turret.trackTarget(robotPoseSupplier, targetSupplier); + return Commands.idle(this).alongWith(turret.trackTarget(robotPoseSupplier, targetSupplier)); + } + + public void setFlywheelVelocity(double velocityRPM) { + flywheel.setVelocity(velocityRPM); + } + + public ShooterSide getSide() { + return side; } public enum ShooterSide { diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java new file mode 100644 index 0000000..eb8d18a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java @@ -0,0 +1,61 @@ +// 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.shooter.flywheel; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.ShooterConstants.FlywheelConstants; +import frc.robot.subsystems.shooter.Shooter.ShooterSide; + +import org.littletonrobotics.junction.Logger; + +public class Flywheel extends SubsystemBase { + private final ShooterSide side; + + private final FlywheelIO io; + private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged(); + + private boolean atGoal = false; + private Debouncer atGoalDebouncer = new Debouncer(0.2, DebounceType.kFalling); + + /** Creates a new Flywheel. */ + public Flywheel(ShooterSide side, FlywheelIO io) { + this.side = side; + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Shooter/" + side.getName() + "/Flywheel", inputs); + Logger.recordOutput("Shooter/" + side.getName() + "/Flywheel/AtGoal", atGoal); + } + + public void setVelocity(double velocityRPM) { + atGoal = atGoalDebouncer.calculate(Math.abs(Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM) + - inputs.velocityRadPerSec) < FlywheelConstants.kSpeedTolerance); + // Rotations per minute -> rotations per second + io.setVelocity(velocityRPM / 60.0); + } + + public void stop() { + io.stop(); + } + + /** + * Gets the current velocity of the flywheel + * + * @return A double representing the speed of the flywheel (in RPM). + */ + public double getVelocity() { + return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec); + } + + public ShooterSide getSide() { + return this.side; + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java new file mode 100644 index 0000000..d4265c3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java @@ -0,0 +1,24 @@ +package frc.robot.subsystems.shooter.flywheel; + +import org.littletonrobotics.junction.AutoLog; + +public interface FlywheelIO { + default void updateInputs(FlywheelIOInputs inputs) {} + + @AutoLog + public static class FlywheelIOInputs { + public boolean connected = false; + public double velocityRadPerSec = 0.0; + public double appliedVolts = 0.0; + public double currentDrawAmps = 0.0; + } + + /** + * Set the shooter motor to a specified velocity. + * + * @param velocity The velocity to set the motor to (in RPM). + */ + default void setVelocity(double velocity) {} + + default void stop() {} +} diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java new file mode 100644 index 0000000..e495b72 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java @@ -0,0 +1,49 @@ +package frc.robot.subsystems.shooter.flywheel; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants; +import frc.robot.Constants.ShooterConstants.FlywheelConstants; + +public class FlywheelIOSim implements FlywheelIO { + private final DCMotor gearbox = DCMotor.getKrakenX44(1); + private final DCMotorSim sim; + + private final PIDController pid = new PIDController(1, 0, 0, Constants.kLoopPeriodSeconds); + + private double appliedVolts = 0.0; + + public FlywheelIOSim() { + sim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(gearbox, 0.025, FlywheelConstants.kGearRatio), + gearbox); + } + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + double currentOutput = pid.calculate(sim.getAngularVelocityRPM()); + appliedVolts = MathUtil.clamp(currentOutput, -12.0, 12.0); + + sim.setInputVoltage(appliedVolts); + sim.update(0.02); + + inputs.connected = true; + inputs.velocityRadPerSec = sim.getAngularVelocityRadPerSec(); + inputs.appliedVolts = appliedVolts; + inputs.currentDrawAmps = sim.getCurrentDrawAmps(); + } + + @Override + public void setVelocity(double velocity) { + pid.setSetpoint(velocity); + } + + @Override + public void stop() { + pid.setSetpoint(0); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java new file mode 100644 index 0000000..56c92e5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -0,0 +1,62 @@ +package frc.robot.subsystems.shooter.flywheel; + +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static frc.robot.util.PhoenixUtil.tryUntilOk; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants.ShooterConstants.FlywheelConstants; + +public class FlywheelIOTalonFX implements FlywheelIO { + private final TalonFX motor; + private final TalonFXConfiguration motorConfig; + + private final StatusSignal velocitySignal; + private final StatusSignal accelerationSignal; + private final StatusSignal voltageSignal; + private final StatusSignal currentSignal; + + private final VelocityVoltage velocityRequest = new VelocityVoltage(0).withSlot(0); + + public FlywheelIOTalonFX(int motorID) { + motor = new TalonFX(motorID); + motorConfig = new TalonFXConfiguration().withSlot0(FlywheelConstants.kGains) + .withMotorOutput(FlywheelConstants.kOutputConfigs); + tryUntilOk(5, () -> motor.getConfigurator().apply(motorConfig, 0.25)); + + velocitySignal = motor.getVelocity(); + accelerationSignal = motor.getAcceleration(); + voltageSignal = motor.getMotorVoltage(); + currentSignal = motor.getStatorCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll(50, velocitySignal, accelerationSignal, voltageSignal, currentSignal); + motor.optimizeBusUtilization(); + } + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + inputs.connected = BaseStatusSignal.refreshAll(velocitySignal, accelerationSignal, voltageSignal, currentSignal) + .isOK(); + inputs.velocityRadPerSec = velocitySignal.getValue().in(RadiansPerSecond); + inputs.appliedVolts = voltageSignal.getValueAsDouble(); + inputs.currentDrawAmps = currentSignal.getValueAsDouble(); + } + + @Override + public void setVelocity(double velocity) { + motor.setControl(velocityRequest.withVelocity(velocity)); + } + + @Override + public void stop() { + motor.stopMotor(); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java b/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java index 5cc4024..63a54cf 100644 --- a/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java +++ b/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java @@ -5,15 +5,43 @@ package frc.robot.subsystems.shooter.hood; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotVisualizer; import frc.robot.subsystems.shooter.Shooter.ShooterSide; +import org.littletonrobotics.junction.Logger; public class Hood extends SubsystemBase { + private final ShooterSide side; + + private final HoodIO io; + private final HoodIOInputsAutoLogged inputs = new HoodIOInputsAutoLogged(); /** Creates a new Hood. */ - public Hood(ShooterSide side, HoodIO io) {} + public Hood(ShooterSide side, HoodIO io) { + this.side = side; + this.io = io; + } @Override public void periodic() { - // This method will be called once per scheduler run + io.updateInputs(inputs); + Logger.processInputs(("Hood/" + side.getName()), inputs); + + if (side == ShooterSide.LEFT) { + RobotVisualizer.getInstance().setLeftHoodAngle(inputs.positionRad); + } else if (side == ShooterSide.RIGHT) { + RobotVisualizer.getInstance().setRightHoodAngle(inputs.positionRad); + } + } + + public double getPosition() { + return inputs.positionRad; + } + + public double getVelocity() { + return inputs.velocityRadPerSec; + } + + public ShooterSide getSide() { + return this.side; } } diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIO.java index 4549488..b81643b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIO.java @@ -7,12 +7,17 @@ default void updateInputs(HoodIOInputs inputs) {} @AutoLog public static class HoodIOInputs { - boolean connected = false; - double angleRads = 0.0; - double velocityRadsPerSec = 0.0; - double appliedVolts = 0.0; - double currentAmps = 0.0; + public boolean connected = false; + public double positionRad = 0.0; + public double velocityRadPerSec = 0.0; + public double appliedVolts = 0.0; + public double currentDrawAmps = 0.0; } + /** + * Sets the target angle for the hood. + * + * @param angle The angle for the hood to aim at (in radians). + */ default void setAngle(double angle) {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSim.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSim.java index 6da98c8..aa0db37 100644 --- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSim.java @@ -3,38 +3,47 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import frc.robot.Constants; import frc.robot.Constants.ShooterConstants.HoodConstants; -import frc.robot.subsystems.shooter.hood.HoodIO.HoodIOInputs; public class HoodIOSim implements HoodIO { private final DCMotor gearbox = DCMotor.getNeo550(1); - private final DCMotorSim sim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(gearbox, 0.025, HoodConstants.kGearRatio), gearbox); - private PIDController pid = new PIDController(1, 0, 0); + private final SingleJointedArmSim sim = + new SingleJointedArmSim( + gearbox, + HoodConstants.kGearRatio, + 0.025, + Units.inchesToMeters(7), + HoodConstants.kMinAngleRad, + HoodConstants.kMaxAngleRad, + true, + 0); + + private final PIDController pid = new PIDController(1.0, 0.0, 0.0, Constants.kLoopPeriodSeconds); + + private double appliedVolts = 0.0; public HoodIOSim() {} @Override public void updateInputs(HoodIOInputs inputs) { - double currentOutput = pid.calculate(sim.getAngularPositionRad() / HoodConstants.kGearRatio); - double volts = MathUtil.clamp(currentOutput, -12.0, 12.0); - - sim.setInputVoltage(volts); + sim.setInputVoltage(appliedVolts); sim.update(0.02); inputs.connected = true; - inputs.angleRads = sim.getAngularPositionRad(); - inputs.velocityRadsPerSec = sim.getAngularVelocityRadPerSec(); - inputs.appliedVolts = volts; - inputs.currentAmps = sim.getCurrentDrawAmps(); + inputs.positionRad = sim.getAngleRads(); + inputs.velocityRadPerSec = sim.getVelocityRadPerSec(); + inputs.appliedVolts = appliedVolts; + inputs.currentDrawAmps = sim.getCurrentDrawAmps(); } @Override public void setAngle(double angle) { - pid.setSetpoint(angle); + angle = MathUtil.clamp(angle, HoodConstants.kMinAngleRad, HoodConstants.kMaxAngleRad); + + appliedVolts = MathUtil.clamp(pid.calculate(sim.getAngleRads(), angle), -12.0, 12.0); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java index a315670..15c497b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java @@ -1,3 +1,12 @@ package frc.robot.subsystems.shooter.hood; -public class HoodIOSparkMax {} +public class HoodIOSparkMax implements HoodIO { + + public HoodIOSparkMax() {} + + @Override + public void updateInputs(HoodIOInputs inputs) { + // TODO Auto-generated method stub + HoodIO.super.updateInputs(inputs); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java index 9a2c316..7cb11d3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java @@ -35,11 +35,10 @@ public void periodic() { io.updateInputs(inputs); Logger.processInputs(("Turret/" + side.getName()), inputs); - if (side.equals(ShooterSide.LEFT)) { - RobotVisualizer.getInstance().setLeftTurretAngle(Rotation2d.fromRadians(inputs.positionRads)); - } else if (side.equals(ShooterSide.RIGHT)) { - RobotVisualizer.getInstance() - .setRightTurretAngle(Rotation2d.fromRadians(inputs.positionRads)); + if (side == ShooterSide.LEFT) { + RobotVisualizer.getInstance().setLeftTurretAngle(Rotation2d.fromRadians(inputs.positionRad)); + } else if (side == ShooterSide.RIGHT) { + RobotVisualizer.getInstance().setRightTurretAngle(Rotation2d.fromRadians(inputs.positionRad)); } Logger.recordOutput(("Turret/" + side.getName() + "/TargetAngle"), targetAngle); @@ -78,7 +77,19 @@ public Command trackTarget( this); } + public void setPosition(Rotation2d position) { + io.setPosition(position); + } + public double getPosition() { - return inputs.positionRads; + return inputs.positionRad; + } + + public double getVelocity() { + return inputs.velocityRadPerSec; + } + + public ShooterSide getSide() { + return this.side; } } diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java index 0c54930..1a5c83b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java @@ -8,11 +8,11 @@ public default void updateInputs(TurretIOInputs inputs) {} @AutoLog public static class TurretIOInputs { - boolean connected = false; - double positionRads = 0.0; - double velocityRadsPerSec = 0.0; - double appliedVolts = 0.0; - double currentAmps = 0.0; + public boolean connected = false; + public double positionRad = 0.0; + public double velocityRadPerSec = 0.0; + public double appliedVolts = 0.0; + public double currentAmps = 0.0; } public default void setPosition(Rotation2d position) {} diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSim.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSim.java index b545c83..dbc5e74 100644 --- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSim.java @@ -6,19 +6,22 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants; import frc.robot.Constants.ShooterConstants.TurretConstants; public class TurretIOSim implements TurretIO { private final DCMotor gearbox = DCMotor.getNEO(1); - private final DCMotorSim sim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(gearbox, 0.025, TurretConstants.kGearRatio), gearbox); + private final DCMotorSim sim; - private PIDController pid = new PIDController(8, 0, 0.3); + private PIDController pid = new PIDController(10, 0, 0.3, Constants.kLoopPeriodSeconds); public TurretIOSim() { pid.reset(); pid.enableContinuousInput(-Math.PI, Math.PI); + sim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(gearbox, 0.025, TurretConstants.kGearRatio), + gearbox); } @Override @@ -30,8 +33,8 @@ public void updateInputs(TurretIOInputs inputs) { sim.update(0.02); inputs.connected = true; - inputs.positionRads = sim.getAngularPositionRad(); - inputs.velocityRadsPerSec = sim.getAngularVelocityRadPerSec(); + inputs.positionRad = sim.getAngularPositionRad(); + inputs.velocityRadPerSec = sim.getAngularVelocityRadPerSec(); inputs.appliedVolts = volts; inputs.currentAmps = sim.getCurrentDrawAmps(); } diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java index 8ff55c0..91eef7a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java @@ -2,7 +2,7 @@ public class TurretIOSparkMax implements TurretIO { - public TurretIOSparkMax(int id) {} + public TurretIOSparkMax() {} @Override public void updateInputs(TurretIOInputs inputs) { From ee3372e8c3a44f6f3df36d6fa3a77248ee07c0bd Mon Sep 17 00:00:00 2001 From: Ryan Hefferon Date: Mon, 9 Feb 2026 17:43:44 -0500 Subject: [PATCH 22/61] bind buttons to run the guts forward and backward as well as both at once, and add a variable to invert the motor depending on what side gut is being created so that both sides run forward with the same command. --- src/main/java/frc/robot/Constants.java | 14 ++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 18 ++++++++++++++---- .../java/frc/robot/subsystems/guts/Guts.java | 11 ++++++----- 3 files changed, 34 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0e0954c..d3440ad 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -53,7 +53,10 @@ import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.MomentOfInertia; import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + import java.util.Map; /** @@ -915,4 +918,15 @@ public class VisionConstants { public static double angularStdDevMegatag2Factor = Double.POSITIVE_INFINITY; // No rotation data available } + public class OperatorConstants { + public static Joystick auxStick = new Joystick(0); + public static JoystickButton leftGutButton1 = new JoystickButton(auxStick, 0); + public static JoystickButton leftGutButton2 = new JoystickButton(auxStick, 1); + public static JoystickButton rightGutButton1 = new JoystickButton(auxStick, 2); + public static JoystickButton rightGutButton2 = new JoystickButton(auxStick, 3); + public static JoystickButton bothGutsButton1 = new JoystickButton(auxStick, 4); + public static JoystickButton bothGutsButton2 = new JoystickButton(auxStick, 5); + + } + } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4796bdf..41671f2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -32,10 +32,10 @@ public class RobotContainer { private final CommandXboxController driver = new CommandXboxController(0); - private final Drive drive; - private final Vision vision; - private final Guts leftGut; - private final Guts rightGut; + private Drive drive; + private Vision vision; + private Guts leftGut; + private Guts rightGut; public RobotContainer() { switch (Constants.kCurrentMode) { @@ -114,6 +114,16 @@ private void configureBindings() { drive, () -> RobotState.getInstance().getEstimatedPose(), () -> Hub.innerCenterPoint.toTranslation2d())); + + Constants.OperatorConstants.leftGutButton1.whileTrue(leftGut.runGutForward()); + Constants.OperatorConstants.leftGutButton2.whileTrue(leftGut.runGutBackward()); + Constants.OperatorConstants.rightGutButton1.whileTrue(rightGut.runGutForward()); + Constants.OperatorConstants.rightGutButton2.whileTrue(rightGut.runGutBackward()); + + Constants.OperatorConstants.bothGutsButton1.whileTrue(leftGut.runGutForward().alongWith(rightGut.runGutForward())); + + Constants.OperatorConstants.bothGutsButton2.whileTrue(leftGut.runGutBackward().alongWith(rightGut.runGutBackward())); + } public void robotPeriodic() { diff --git a/src/main/java/frc/robot/subsystems/guts/Guts.java b/src/main/java/frc/robot/subsystems/guts/Guts.java index b5a8d96..a25059b 100644 --- a/src/main/java/frc/robot/subsystems/guts/Guts.java +++ b/src/main/java/frc/robot/subsystems/guts/Guts.java @@ -21,6 +21,7 @@ public class Guts extends SubsystemBase { private final GutSide side; public final GutsIO io; public GutsIOInputsAutoLogged inputs = new GutsIOInputsAutoLogged(); + public double speed = (side == GutSide.LEFT) ? 0.5 : -0.5; /** Creates a new Guts. */ public Guts(GutSide side, GutsIO io) { @@ -33,17 +34,17 @@ public Guts(GutSide side, GutsIO io) { */ public Command runGutForward() { return Commands.runEnd( - () -> io.setGutMotorSpeed(0.5), - () -> io.setGutMotorSpeed(0), - this); + () -> io.setGutMotorSpeed(speed), + () -> io.setGutMotorSpeed(0), + this); } /** * Runs the gut motor backward at 0.5 speed, then stops it when finished. */ public Command runGutBackward() { - return Commands.runEnd( - () -> io.setGutMotorSpeed(-0.5), + return Commands.runEnd( + () -> io.setGutMotorSpeed(-speed), () -> io.setGutMotorSpeed(0), this); } From 30699eedd8aa1a688e9f419d948a1106fdd3fce9 Mon Sep 17 00:00:00 2001 From: Maxwell Morgan Date: Tue, 10 Feb 2026 15:50:22 -0500 Subject: [PATCH 23/61] Add hardware for turret --- src/main/java/frc/robot/Constants.java | 765 +++++++++--------- src/main/java/frc/robot/RobotVisualizer.java | 1 - .../shooter/TrajectoryCalculator.java | 36 +- .../subsystems/shooter/flywheel/Flywheel.java | 11 +- .../shooter/flywheel/FlywheelIOSim.java | 2 +- .../shooter/flywheel/FlywheelIOTalonFX.java | 92 ++- .../shooter/turret/TurretIOSparkMax.java | 81 +- src/main/java/frc/robot/util/SparkUtil.java | 56 ++ 8 files changed, 622 insertions(+), 422 deletions(-) create mode 100644 src/main/java/frc/robot/util/SparkUtil.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a7374ef..fc3a911 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -53,388 +53,419 @@ import frc.robot.util.GeomUtil; /** - * This class defines the runtime mode used by AdvantageKit. The mode is always - * "real" when running - * on a roboRIO. Change the value of "simMode" to switch between "sim" (physics - * sim) and "replay" + * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running + * on a roboRIO. Change the value of "simMode" to switch between "sim" (physics sim) and "replay" * (log replay from a file). */ public final class Constants { - public static final double kLoopPeriodSeconds = 0.02; + public static final double kLoopPeriodSeconds = 0.02; + + public static final Mode kSimMode = Mode.SIM; + public static final Mode kCurrentMode = RobotBase.isReal() ? Mode.REAL : kSimMode; + + public static enum Mode { + /** Running on a real robot. */ + REAL, + + /** Running a physics simulator. */ + SIM, + + /** Replaying from a log file. */ + REPLAY + } + + public static final int kDriverControllerPort = 0; + public static final int kAuxControllerPort = 1; + + public static boolean kDisableHAL = false; + + public static void disableHAL() { + kDisableHAL = true; + } + + public static final class DriveConstants { + public static final SwerveDriveKinematics kSwerveKinematics = + new SwerveDriveKinematics(Drive.getModuleTranslations()); + + public static final double kOdometryFrequency = + ModuleConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0; + public static final double kDriveBaseRadius = + Math.max( + Math.max( + Math.hypot( + ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), + Math.hypot( + ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY)), + Math.max( + Math.hypot(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), + Math.hypot( + ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY))); + + public static final Translation2d[] kModuleTranslations = + new Translation2d[] { + new Translation2d( + ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), + new Translation2d( + ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY), + new Translation2d(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), + new Translation2d( + ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY) + }; - public static final Mode kSimMode = Mode.SIM; - public static final Mode kCurrentMode = RobotBase.isReal() ? Mode.REAL : kSimMode; + // TODO: Update for robot + // PathPlanner config constants + public static final double kRobotMassKG = 74.088; + public static final double kRobotMOI = 6.883; + /** Coefficient of friction */ + public static final double kWheelCOF = 1.2; + + public static final RobotConfig kPathplannerConfig = + new RobotConfig( + kRobotMOI, + kRobotMOI, + new ModuleConfig( + ModuleConstants.FrontLeft.WheelRadius, + ModuleConstants.kSpeedAt12Volts.in(MetersPerSecond), + kWheelCOF, + DCMotor.getKrakenX60(1) + .withReduction(ModuleConstants.FrontLeft.DriveMotorGearRatio), + ModuleConstants.FrontLeft.SlipCurrent, + 1), + kModuleTranslations); + + public static final class ModuleConstants { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with + // the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + // TODO: Update for robot + private static final Slot0Configs steerGains = + new Slot0Configs() + .withKP(100) + .withKI(0) + .withKD(0.5) + .withKS(0.1) + .withKV(1.91) + .withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + // TODO: Update for robot + private static final Slot0Configs driveGains = + new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = + ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = + ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = + DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = + SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to + // RemoteCANcoder + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + // TODO: Update for robot + private static final Current kSlipCurrent = Amps.of(120.0); + + // Initial configs for the drive and steer motors and the azimuth encoder; these + // cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API + // documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a + // relatively + // low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true)); + private static final CANcoderConfiguration encoderInitialConfigs = + new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + // TODO: Update for robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + // TODO: Update for robot + private static final double kCoupleRatio = 3.8181818181818183; + // TODO: Update for robot + private static final double kDriveGearRatio = 7.363636363636365; + private static final double kSteerGearRatio = 15.42857142857143; + private static final Distance kWheelRadius = Inches.of(2.167); + // TODO: Update for robot + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + // TODO: Update for robot + private static final int kPigeonId = 1; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = + new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + ConstantCreator = + new SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + // TODO: Update for robot + // Front Left + private static final int kFrontLeftDriveMotorId = 3; + private static final int kFrontLeftSteerMotorId = 2; + private static final int kFrontLeftEncoderId = 1; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(10); + private static final Distance kFrontLeftYPos = Inches.of(10); + // TODO: Update for robot + // Front Right + private static final int kFrontRightDriveMotorId = 1; + private static final int kFrontRightSteerMotorId = 0; + private static final int kFrontRightEncoderId = 0; + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(10); + private static final Distance kFrontRightYPos = Inches.of(-10); + // TODO: Update for robot + // Back Left + private static final int kBackLeftDriveMotorId = 7; + private static final int kBackLeftSteerMotorId = 6; + private static final int kBackLeftEncoderId = 3; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); + private static final boolean kBackLeftSteerMotorInverted = true; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-10); + private static final Distance kBackLeftYPos = Inches.of(10); + // TODO: Update for robot + // Back Right + private static final int kBackRightDriveMotorId = 5; + private static final int kBackRightSteerMotorId = 4; + private static final int kBackRightEncoderId = 2; + private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-10); + private static final Distance kBackRightYPos = Inches.of(-10); + + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, + kFrontLeftDriveMotorId, + kFrontLeftEncoderId, + kFrontLeftEncoderOffset, + kFrontLeftXPos, + kFrontLeftYPos, + kInvertLeftSide, + kFrontLeftSteerMotorInverted, + kFrontLeftEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, + kFrontRightDriveMotorId, + kFrontRightEncoderId, + kFrontRightEncoderOffset, + kFrontRightXPos, + kFrontRightYPos, + kInvertRightSide, + kFrontRightSteerMotorInverted, + kFrontRightEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, + kBackLeftDriveMotorId, + kBackLeftEncoderId, + kBackLeftEncoderOffset, + kBackLeftXPos, + kBackLeftYPos, + kInvertLeftSide, + kBackLeftSteerMotorInverted, + kBackLeftEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, + kBackRightDriveMotorId, + kBackRightEncoderId, + kBackRightEncoderOffset, + kBackRightXPos, + kBackRightYPos, + kInvertRightSide, + kBackRightSteerMotorInverted, + kBackRightEncoderInverted); + } + } + + public static final class VisionConstants { + // AprilTag layout + public static AprilTagFieldLayout aprilTagLayout = + AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + + // Camera names, must match names configured on coprocessor + public static String camera0Name = "camera_0"; + public static String camera1Name = "camera_1"; + + // Robot to camera transforms + // (Not used by Limelight, configure in web UI instead) + public static Transform3d robotToCamera0 = + new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); + public static Transform3d robotToCamera1 = + new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); + + // Basic filtering thresholds + public static double maxAmbiguity = 0.3; + public static double maxZError = 0.75; + + // Standard deviation baselines, for 1 meter distance and 1 tag + // (Adjusted automatically based on distance and # of tags) + public static double linearStdDevBaseline = 0.02; // Meters + public static double angularStdDevBaseline = 0.06; // Radians + + // Standard deviation multipliers for each camera + // (Adjust to trust some cameras more than others) + public static double[] cameraStdDevFactors = + new double[] { + 1.0, // Camera 0 + 1.0 // Camera 1 + }; - public static enum Mode { - /** Running on a real robot. */ - REAL, + // Multipliers to apply for MegaTag 2 observations + public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve + public static double angularStdDevMegatag2Factor = + Double.POSITIVE_INFINITY; // No rotation data available + } - /** Running a physics simulator. */ - SIM, + public static final class ShooterConstants { - /** Replaying from a log file. */ - REPLAY - } + public static final class TurretConstants { + public static final double kGearRatio = 10 / 1; + public static final double kMinTurretAngleRad = Units.degreesToRadians(-180); + public static final double kMaxTurretAngleRad = Units.degreesToRadians(180); - public static final int kDriverControllerPort = 0; - public static final int kAuxControllerPort = 1; + public static final double kLeftMotorId = 12; + public static final double kRightMotorId = 13; - public static boolean kDisableHAL = false; + // +X = Forward, +Y = Left + public static final Transform3d kRobotToLeftTurret = + new Transform3d(Inches.of(3.749), Inches.of(8.186), Inches.of(13.401), Rotation3d.kZero); - public static void disableHAL() { - kDisableHAL = true; + public static final Transform3d kRobotToRightTurret = + new Transform3d(Inches.of(3.749), Inches.of(-8.314), Inches.of(13.401), Rotation3d.kZero); } - public static final class DriveConstants { - public static final SwerveDriveKinematics kSwerveKinematics = new SwerveDriveKinematics( - Drive.getModuleTranslations()); - - public static final double kOdometryFrequency = ModuleConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0; - public static final double kDriveBaseRadius = Math.max( - Math.max( - Math.hypot( - ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), - Math.hypot( - ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY)), - Math.max( - Math.hypot(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), - Math.hypot( - ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY))); - - public static final Translation2d[] kModuleTranslations = new Translation2d[] { - new Translation2d( - ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), - new Translation2d( - ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY), - new Translation2d(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), - new Translation2d( - ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY) - }; - - // TODO: Update for robot - // PathPlanner config constants - public static final double kRobotMassKG = 74.088; - public static final double kRobotMOI = 6.883; - /** Coefficient of friction */ - public static final double kWheelCOF = 1.2; - - public static final RobotConfig kPathplannerConfig = new RobotConfig( - kRobotMOI, - kRobotMOI, - new ModuleConfig( - ModuleConstants.FrontLeft.WheelRadius, - ModuleConstants.kSpeedAt12Volts.in(MetersPerSecond), - kWheelCOF, - DCMotor.getKrakenX60(1) - .withReduction(ModuleConstants.FrontLeft.DriveMotorGearRatio), - ModuleConstants.FrontLeft.SlipCurrent, - 1), - kModuleTranslations); - - public static final class ModuleConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with - // the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - // TODO: Update for robot - private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100) - .withKI(0) - .withKD(0.5) - .withKS(0.1) - .withKV(1.91) - .withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - // TODO: Update for robot - private static final Slot0Configs driveGains = new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0) - .withKV(0.124); - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to - // RemoteCANcoder - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - // TODO: Update for robot - private static final Current kSlipCurrent = Amps.of(120.0); - - // Initial configs for the drive and steer motors and the azimuth encoder; these - // cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API - // documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a - // relatively - // low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true)); - private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; - - // CAN bus that the devices are located on; - // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot"); - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - // TODO: Update for robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - // TODO: Update for robot - private static final double kCoupleRatio = 3.8181818181818183; - // TODO: Update for robot - private static final double kDriveGearRatio = 7.363636363636365; - private static final double kSteerGearRatio = 15.42857142857143; - private static final Distance kWheelRadius = Inches.of(2.167); - // TODO: Update for robot - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - // TODO: Update for robot - private static final int kPigeonId = 1; - - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - - public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - - // TODO: Update for robot - // Front Left - private static final int kFrontLeftDriveMotorId = 3; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 1; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); - private static final boolean kFrontLeftSteerMotorInverted = true; - private static final boolean kFrontLeftEncoderInverted = false; - - private static final Distance kFrontLeftXPos = Inches.of(10); - private static final Distance kFrontLeftYPos = Inches.of(10); - // TODO: Update for robot - // Front Right - private static final int kFrontRightDriveMotorId = 1; - private static final int kFrontRightSteerMotorId = 0; - private static final int kFrontRightEncoderId = 0; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); - private static final boolean kFrontRightSteerMotorInverted = true; - private static final boolean kFrontRightEncoderInverted = false; - - private static final Distance kFrontRightXPos = Inches.of(10); - private static final Distance kFrontRightYPos = Inches.of(-10); - // TODO: Update for robot - // Back Left - private static final int kBackLeftDriveMotorId = 7; - private static final int kBackLeftSteerMotorId = 6; - private static final int kBackLeftEncoderId = 3; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); - private static final boolean kBackLeftSteerMotorInverted = true; - private static final boolean kBackLeftEncoderInverted = false; - - private static final Distance kBackLeftXPos = Inches.of(-10); - private static final Distance kBackLeftYPos = Inches.of(10); - // TODO: Update for robot - // Back Right - private static final int kBackRightDriveMotorId = 5; - private static final int kBackRightSteerMotorId = 4; - private static final int kBackRightEncoderId = 2; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); - private static final boolean kBackRightSteerMotorInverted = true; - private static final boolean kBackRightEncoderInverted = false; - - private static final Distance kBackRightXPos = Inches.of(-10); - private static final Distance kBackRightYPos = Inches.of(-10); - - public static final SwerveModuleConstants FrontLeft = ConstantCreator - .createModuleConstants( - kFrontLeftSteerMotorId, - kFrontLeftDriveMotorId, - kFrontLeftEncoderId, - kFrontLeftEncoderOffset, - kFrontLeftXPos, - kFrontLeftYPos, - kInvertLeftSide, - kFrontLeftSteerMotorInverted, - kFrontLeftEncoderInverted); - public static final SwerveModuleConstants FrontRight = ConstantCreator - .createModuleConstants( - kFrontRightSteerMotorId, - kFrontRightDriveMotorId, - kFrontRightEncoderId, - kFrontRightEncoderOffset, - kFrontRightXPos, - kFrontRightYPos, - kInvertRightSide, - kFrontRightSteerMotorInverted, - kFrontRightEncoderInverted); - public static final SwerveModuleConstants BackLeft = ConstantCreator - .createModuleConstants( - kBackLeftSteerMotorId, - kBackLeftDriveMotorId, - kBackLeftEncoderId, - kBackLeftEncoderOffset, - kBackLeftXPos, - kBackLeftYPos, - kInvertLeftSide, - kBackLeftSteerMotorInverted, - kBackLeftEncoderInverted); - public static final SwerveModuleConstants BackRight = ConstantCreator - .createModuleConstants( - kBackRightSteerMotorId, - kBackRightDriveMotorId, - kBackRightEncoderId, - kBackRightEncoderOffset, - kBackRightXPos, - kBackRightYPos, - kInvertRightSide, - kBackRightSteerMotorInverted, - kBackRightEncoderInverted); - } + public static final class HoodConstants { + public static final double kTurretToHoodInches = 1.878; + public static final double kGearRatio = 100 / 1; + + public static final Transform3d kRobotToLeftHood = + new Transform3d( + Inches.of(7.268715), Meters.of(0.20792316), Inches.of(16.018516), Rotation3d.kZero); + + public static final Transform3d kRobotToRightHood = + new Transform3d( + Inches.of(-7.270121), + Inches.of(-(12.062888 - (7.5 / 2.0))), + Inches.of(16.018516), + Rotation3d.kZero); + + public static final Transform3d kLeftTurretToLeftHood = + GeomUtil.toPose3d(HoodConstants.kRobotToLeftHood) + .minus( + GeomUtil.toPose3d(TurretConstants.kRobotToLeftTurret) + .plus( + new Transform3d( + Inches.of(7.268715), Inches.of(0), Inches.of(0), new Rotation3d()))); + + public static final Transform3d kRightTurretToRightHood = + GeomUtil.toPose3d(HoodConstants.kRobotToRightHood) + .minus( + GeomUtil.toPose3d(TurretConstants.kRobotToRightTurret) + .plus( + new Transform3d( + Inches.of(-7.270121), Inches.of(0), Inches.of(0), new Rotation3d()))); + + public static final double kMinAngleRad = Units.degreesToRadians(0); + public static final double kMaxAngleRad = Units.degreesToRadians(40); } - public static final class VisionConstants { - // AprilTag layout - public static AprilTagFieldLayout aprilTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); - - // Camera names, must match names configured on coprocessor - public static String camera0Name = "camera_0"; - public static String camera1Name = "camera_1"; - - // Robot to camera transforms - // (Not used by Limelight, configure in web UI instead) - public static Transform3d robotToCamera0 = new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); - public static Transform3d robotToCamera1 = new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); - - // Basic filtering thresholds - public static double maxAmbiguity = 0.3; - public static double maxZError = 0.75; - - // Standard deviation baselines, for 1 meter distance and 1 tag - // (Adjusted automatically based on distance and # of tags) - public static double linearStdDevBaseline = 0.02; // Meters - public static double angularStdDevBaseline = 0.06; // Radians - - // Standard deviation multipliers for each camera - // (Adjust to trust some cameras more than others) - public static double[] cameraStdDevFactors = new double[] { - 1.0, // Camera 0 - 1.0 // Camera 1 - }; + public static final class FlywheelConstants { + public static final double kGearRatio = 300; + public static final double kSpeedTolerance = 25.0; - // Multipliers to apply for MegaTag 2 observations - public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve - public static double angularStdDevMegatag2Factor = Double.POSITIVE_INFINITY; // No rotation data available - } + public static final int kLeftFlywheelID = 2; - public static final class ShooterConstants { - - public static final class TurretConstants { - public static final double kGearRatio = 10 / 1; - public static final double kMinTurretAngleRad = -3.0 * Math.PI / 2.0; // -270 degrees - public static final double kMaxTurretAngleRad = 3.0 * Math.PI / 2.0; // +270 degrees - - public static final double kLeftMotorId = 12; - public static final double kRightMotorId = 13; - - // +X = Forward, +Y = Left - public static final Transform3d kRobotToLeftTurret = new Transform3d(Inches.of(3.749), Inches.of(8.186), - Inches.of(13.401), Rotation3d.kZero); - - public static final Transform3d kRobotToRightTurret = new Transform3d(Inches.of(3.749), Inches.of(-8.314), - Inches.of(13.401), Rotation3d.kZero); - } - - public static final class HoodConstants { - public static final double kTurretToHoodInches = 1.878; - public static final double kGearRatio = 100 / 1; - - public static final Transform3d kRobotToLeftHood = new Transform3d( - Inches.of(7.268715), Meters.of(0.20792316), Inches.of(16.018516), Rotation3d.kZero); - - public static final Transform3d kRobotToRightHood = new Transform3d( - Inches.of(-7.270121), - Inches.of(-(12.062888 - (7.5 / 2.0))), - Inches.of(16.018516), - Rotation3d.kZero); - - public static final Transform3d kLeftTurretToLeftHood = GeomUtil.toPose3d(HoodConstants.kRobotToLeftHood) - .minus( - GeomUtil.toPose3d(TurretConstants.kRobotToLeftTurret) - .plus( - new Transform3d( - Inches.of(7.268715), Inches.of(0), Inches.of(0), - new Rotation3d()))); - - public static final Transform3d kRightTurretToRightHood = GeomUtil.toPose3d(HoodConstants.kRobotToRightHood) - .minus( - GeomUtil.toPose3d(TurretConstants.kRobotToRightTurret) - .plus( - new Transform3d( - Inches.of(-7.270121), Inches.of(0), Inches.of(0), - new Rotation3d()))); - - public static final double kMinAngleRad = Units.degreesToRadians(0); - public static final double kMaxAngleRad = Units.degreesToRadians(40); - } - - public static final class FlywheelConstants { - public static final double kGearRatio = 300; - public static final double kSpeedTolerance = 25.0; - - public static final int kLeftFlywheelID = 2; - - public static final Slot0Configs kGains = new Slot0Configs().withKP(1).withKD(0).withKS(0); - public static final MotorOutputConfigs kOutputConfigs = new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Coast).withInverted(InvertedValue.Clockwise_Positive); - } + public static final Slot0Configs kGains = new Slot0Configs().withKP(1).withKD(0).withKS(0); + public static final MotorOutputConfigs kOutputConfigs = + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast) + .withInverted(InvertedValue.Clockwise_Positive); } + } } diff --git a/src/main/java/frc/robot/RobotVisualizer.java b/src/main/java/frc/robot/RobotVisualizer.java index 69b639f..bc6a90a 100644 --- a/src/main/java/frc/robot/RobotVisualizer.java +++ b/src/main/java/frc/robot/RobotVisualizer.java @@ -5,7 +5,6 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.Timer; import frc.robot.Constants.ShooterConstants.HoodConstants; import frc.robot.Constants.ShooterConstants.TurretConstants; import frc.robot.util.GeomUtil; diff --git a/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java index c60fd7c..cb13ac9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java @@ -1,8 +1,40 @@ package frc.robot.subsystems.shooter; -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.interpolation.InterpolatingTreeMap; +import edu.wpi.first.math.interpolation.InverseInterpolator; public class TrajectoryCalculator { + private static final InterpolatingTreeMap shooterTable = + new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), ShooterParams::interpolate); - public record ShooterParams(double wheelRPM, double hoodAngle, Rotation2d turretAngle) {} + static { + shooterTable.put(1.5, new ShooterParams(2800.0, 35.0)); + shooterTable.put(2.0, new ShooterParams(3100.0, 38.0)); + shooterTable.put(2.5, new ShooterParams(3400.0, 42.0)); + shooterTable.put(3.0, new ShooterParams(3650.0, 46.0)); + shooterTable.put(3.5, new ShooterParams(3900.0, 50.0)); + shooterTable.put(4.0, new ShooterParams(4100.0, 54.0)); + shooterTable.put(4.5, new ShooterParams(4350.0, 58.0)); + shooterTable.put(5.0, new ShooterParams(4550.0, 62.0)); + } + + // public static ShooterParams calculate(Supplier robotPoseSupplier, + // Supplier robotSpeeds) { + + // Pose2d currPose = robotPoseSupplier.get(); + // ChassisSpeeds robotRelativeVel = robotSpeeds.get(); + + // } + + public record ShooterParams(double wheelRPM, double hoodAngle) + implements Interpolatable { + + @Override + public ShooterParams interpolate(ShooterParams other, double t) { + return new ShooterParams( + wheelRPM + (other.wheelRPM - wheelRPM) * t, + hoodAngle + (other.hoodAngle - hoodAngle) * t); + } + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java index eb8d18a..92ef657 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java @@ -10,7 +10,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ShooterConstants.FlywheelConstants; import frc.robot.subsystems.shooter.Shooter.ShooterSide; - import org.littletonrobotics.junction.Logger; public class Flywheel extends SubsystemBase { @@ -36,8 +35,12 @@ public void periodic() { } public void setVelocity(double velocityRPM) { - atGoal = atGoalDebouncer.calculate(Math.abs(Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM) - - inputs.velocityRadPerSec) < FlywheelConstants.kSpeedTolerance); + atGoal = + atGoalDebouncer.calculate( + Math.abs( + Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM) + - inputs.velocityRadPerSec) + < FlywheelConstants.kSpeedTolerance); // Rotations per minute -> rotations per second io.setVelocity(velocityRPM / 60.0); } @@ -48,7 +51,7 @@ public void stop() { /** * Gets the current velocity of the flywheel - * + * * @return A double representing the speed of the flywheel (in RPM). */ public double getVelocity() { diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java index e495b72..659dbf2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java @@ -44,6 +44,6 @@ public void setVelocity(double velocity) { @Override public void stop() { - pid.setSetpoint(0); + pid.setSetpoint(0); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java index 56c92e5..5f510a7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -8,7 +8,6 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; - import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; @@ -16,47 +15,52 @@ import frc.robot.Constants.ShooterConstants.FlywheelConstants; public class FlywheelIOTalonFX implements FlywheelIO { - private final TalonFX motor; - private final TalonFXConfiguration motorConfig; - - private final StatusSignal velocitySignal; - private final StatusSignal accelerationSignal; - private final StatusSignal voltageSignal; - private final StatusSignal currentSignal; - - private final VelocityVoltage velocityRequest = new VelocityVoltage(0).withSlot(0); - - public FlywheelIOTalonFX(int motorID) { - motor = new TalonFX(motorID); - motorConfig = new TalonFXConfiguration().withSlot0(FlywheelConstants.kGains) - .withMotorOutput(FlywheelConstants.kOutputConfigs); - tryUntilOk(5, () -> motor.getConfigurator().apply(motorConfig, 0.25)); - - velocitySignal = motor.getVelocity(); - accelerationSignal = motor.getAcceleration(); - voltageSignal = motor.getMotorVoltage(); - currentSignal = motor.getStatorCurrent(); - - BaseStatusSignal.setUpdateFrequencyForAll(50, velocitySignal, accelerationSignal, voltageSignal, currentSignal); - motor.optimizeBusUtilization(); - } - - @Override - public void updateInputs(FlywheelIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(velocitySignal, accelerationSignal, voltageSignal, currentSignal) - .isOK(); - inputs.velocityRadPerSec = velocitySignal.getValue().in(RadiansPerSecond); - inputs.appliedVolts = voltageSignal.getValueAsDouble(); - inputs.currentDrawAmps = currentSignal.getValueAsDouble(); - } - - @Override - public void setVelocity(double velocity) { - motor.setControl(velocityRequest.withVelocity(velocity)); - } - - @Override - public void stop() { - motor.stopMotor(); - } + private final TalonFX motor; + private final TalonFXConfiguration motorConfig; + + private final StatusSignal velocitySignal; + private final StatusSignal accelerationSignal; + private final StatusSignal voltageSignal; + private final StatusSignal currentSignal; + + private final VelocityVoltage velocityRequest = new VelocityVoltage(0).withSlot(0); + + public FlywheelIOTalonFX(int motorID) { + motor = new TalonFX(motorID); + motorConfig = + new TalonFXConfiguration() + .withSlot0(FlywheelConstants.kGains) + .withMotorOutput(FlywheelConstants.kOutputConfigs); + tryUntilOk(5, () -> motor.getConfigurator().apply(motorConfig, 0.25)); + + velocitySignal = motor.getVelocity(); + accelerationSignal = motor.getAcceleration(); + voltageSignal = motor.getMotorVoltage(); + currentSignal = motor.getStatorCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50, velocitySignal, accelerationSignal, voltageSignal, currentSignal); + motor.optimizeBusUtilization(); + } + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + inputs.connected = + BaseStatusSignal.refreshAll( + velocitySignal, accelerationSignal, voltageSignal, currentSignal) + .isOK(); + inputs.velocityRadPerSec = velocitySignal.getValue().in(RadiansPerSecond); + inputs.appliedVolts = voltageSignal.getValueAsDouble(); + inputs.currentDrawAmps = currentSignal.getValueAsDouble(); + } + + @Override + public void setVelocity(double velocity) { + motor.setControl(velocityRequest.withVelocity(velocity)); + } + + @Override + public void stop() { + motor.stopMotor(); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java index 91eef7a..3369cb3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java @@ -1,12 +1,87 @@ package frc.robot.subsystems.shooter.turret; +import static frc.robot.util.SparkUtil.ifOk; +import static frc.robot.util.SparkUtil.sparkStickyFault; +import static frc.robot.util.SparkUtil.tryUntilOk; + +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkMaxConfig; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.Constants.ShooterConstants.TurretConstants; + +import java.util.function.DoubleSupplier; + public class TurretIOSparkMax implements TurretIO { + private final SparkMax motor; + private final RelativeEncoder encoder; + private final SparkClosedLoopController motorController; + + private final Debouncer connectedDebouncer = new Debouncer(0.5, DebounceType.kFalling); + + public TurretIOSparkMax(int motorID) { + motor = new SparkMax(motorID, MotorType.kBrushless); + encoder = motor.getEncoder(); + motorController = motor.getClosedLoopController(); + + SparkMaxConfig config = new SparkMaxConfig(); + + config.idleMode(SparkMaxConfig.IdleMode.kBrake); + // .smartCurrentLimit(30); + + config.encoder + .positionConversionFactor(2 * Math.PI / TurretConstants.kGearRatio) // No absolute encoder... + .velocityConversionFactor(2 * Math.PI / TurretConstants.kGearRatio / 60.0); - public TurretIOSparkMax() {} + config.closedLoop + .pid(2.0, 0.0, 0.1) + .positionWrappingEnabled(false) + .feedbackSensor(FeedbackSensor.kPrimaryEncoder); + + config.softLimit + .reverseSoftLimit(TurretConstants.kMinTurretAngleRad) + .forwardSoftLimit(TurretConstants.kMaxTurretAngleRad) + .reverseSoftLimitEnabled(true) + .forwardSoftLimitEnabled(true); + + config.closedLoop.feedForward + .kS(0); + + tryUntilOk( + motor, + 5, + () -> motor.configure( + config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + tryUntilOk(motor, 5, () -> encoder.setPosition(0)); + } @Override public void updateInputs(TurretIOInputs inputs) { - // TODO Auto-generated method stub - TurretIO.super.updateInputs(inputs); + sparkStickyFault = false; + ifOk(motor, encoder::getPosition, (value) -> inputs.positionRad = value); + ifOk(motor, encoder::getVelocity, (value) -> inputs.velocityRadPerSec = value); + ifOk( + motor, + new DoubleSupplier[] { motor::getAppliedOutput, motor::getBusVoltage }, + (values) -> inputs.appliedVolts = values[0] * values[1]); + ifOk(motor, motor::getOutputCurrent, (value) -> inputs.currentAmps = value); + inputs.connected = connectedDebouncer.calculate(!sparkStickyFault); + } + + @Override + public void setPosition(Rotation2d position) { + double clampedPosition = MathUtil.clamp(position.getRadians(), TurretConstants.kMinTurretAngleRad, TurretConstants.kMaxTurretAngleRad); + + motorController.setSetpoint(clampedPosition, ControlType.kPosition); } } diff --git a/src/main/java/frc/robot/util/SparkUtil.java b/src/main/java/frc/robot/util/SparkUtil.java new file mode 100644 index 0000000..8dbe477 --- /dev/null +++ b/src/main/java/frc/robot/util/SparkUtil.java @@ -0,0 +1,56 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.util; + +import com.revrobotics.REVLibError; +import com.revrobotics.spark.SparkBase; +import java.util.function.Consumer; +import java.util.function.DoubleConsumer; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class SparkUtil { + /** Stores whether any error was has been detected by other utility methods. */ + public static boolean sparkStickyFault = false; + + /** Processes a value from a Spark only if the value is valid. */ + public static void ifOk(SparkBase spark, DoubleSupplier supplier, DoubleConsumer consumer) { + double value = supplier.getAsDouble(); + if (spark.getLastError() == REVLibError.kOk) { + consumer.accept(value); + } else { + sparkStickyFault = true; + } + } + + /** Processes a value from a Spark only if the value is valid. */ + public static void ifOk( + SparkBase spark, DoubleSupplier[] suppliers, Consumer consumer) { + double[] values = new double[suppliers.length]; + for (int i = 0; i < suppliers.length; i++) { + values[i] = suppliers[i].getAsDouble(); + if (spark.getLastError() != REVLibError.kOk) { + sparkStickyFault = true; + return; + } + } + consumer.accept(values); + } + + /** Attempts to run the command until no error is produced. */ + public static void tryUntilOk(SparkBase spark, int maxAttempts, Supplier command) { + for (int i = 0; i < maxAttempts; i++) { + var error = command.get(); + if (error == REVLibError.kOk) { + break; + } else { + sparkStickyFault = true; + } + } + } +} From 1637bee4afd0b978e5abbaa7023574f774867765 Mon Sep 17 00:00:00 2001 From: Ryan Hefferon Date: Tue, 10 Feb 2026 17:19:54 -0500 Subject: [PATCH 24/61] add constants for all object ids, motor speeds, and gear ratios. --- src/main/java/frc/robot/Constants.java | 10 ++++++++++ src/main/java/frc/robot/subsystems/guts/Guts.java | 3 ++- .../java/frc/robot/subsystems/guts/GutsIOSparkMax.java | 4 +++- 3 files changed, 15 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d3440ad..a2af145 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -929,4 +929,14 @@ public class OperatorConstants { } + public static final class GutsConstants { + + public static final int gutMotorID = 0; + + public static final double gutMotorSpeed = 0.5; + + //Change Gear Ratio later + public static final double gutMotorGearRatio = 0.0; + } + } diff --git a/src/main/java/frc/robot/subsystems/guts/Guts.java b/src/main/java/frc/robot/subsystems/guts/Guts.java index a25059b..2a3b0d0 100644 --- a/src/main/java/frc/robot/subsystems/guts/Guts.java +++ b/src/main/java/frc/robot/subsystems/guts/Guts.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.GutsConstants; /** * This class updates and stores the values of the inputs periodically, and @@ -21,7 +22,7 @@ public class Guts extends SubsystemBase { private final GutSide side; public final GutsIO io; public GutsIOInputsAutoLogged inputs = new GutsIOInputsAutoLogged(); - public double speed = (side == GutSide.LEFT) ? 0.5 : -0.5; + public double speed = (side == GutSide.LEFT) ? (GutsConstants.gutMotorSpeed) : -(GutsConstants.gutMotorSpeed); /** Creates a new Guts. */ public Guts(GutSide side, GutsIO io) { diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java b/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java index 31c7940..ea91087 100644 --- a/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java @@ -7,6 +7,8 @@ import com.revrobotics.PersistMode; import com.revrobotics.spark.config.SparkMaxConfig; +import frc.robot.Constants.GutsConstants; + /** * This class contains all of the physical objects: one motor and its * corresponding encoder. It also implements the default methods specified in @@ -17,7 +19,7 @@ */ public class GutsIOSparkMax implements GutsIO { - private final SparkMax GutMotor = new SparkMax(0, MotorType.kBrushless); + private final SparkMax GutMotor = new SparkMax(GutsConstants.gutMotorID, MotorType.kBrushless); private final RelativeEncoder GutEncoder = GutMotor.getEncoder(); private final SparkMaxConfig GutMotorConfig; From be8db3bec0dfddef5db9c212c4eb3984c171664b Mon Sep 17 00:00:00 2001 From: Ryan Hefferon Date: Tue, 10 Feb 2026 17:24:31 -0500 Subject: [PATCH 25/61] add constants for all object ids, motor speeds, and gear ratios. --- src/main/java/frc/robot/Constants.java | 7 +++++++ .../java/frc/robot/subsystems/intake/IntakeIOHardware.java | 5 +++-- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b72fd87..e21d25a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -919,5 +919,12 @@ public class VisionConstants { public static class IntakeConstants { public static final int kPivotMotorID = 8; public static final int kRollerMotorID = 9; + + public static final double kPivotMotorSpeed = 0.5; + public static final double kRollerMotorSpeed = 0.5; + + //Change Gear Ratios later + public static final double kPivotMotorGearRatio = 0.0; + public static final double kRollerMotorGearRatio = 0.0; } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java index 1c8a3fa..89903e3 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java @@ -7,10 +7,11 @@ import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.wpilibj.DigitalInput; +import frc.robot.Constants.IntakeConstants; public class IntakeIOHardware implements IntakeIO { - SparkMax armMotor = new SparkMax(5, MotorType.kBrushless); - SparkMax wheelMotor = new SparkMax(6, MotorType.kBrushless); + SparkMax armMotor = new SparkMax(IntakeConstants.kPivotMotorID, MotorType.kBrushless); + SparkMax wheelMotor = new SparkMax(IntakeConstants.kRollerMotorID, MotorType.kBrushless); RelativeEncoder armEncoder = armMotor.getEncoder(); RelativeEncoder wheelEncoder = wheelMotor.getEncoder(); SparkMaxConfig armConfig; From 80f4abefd8386f2112de6e812b436dee238253ef Mon Sep 17 00:00:00 2001 From: Ryan Hefferon Date: Tue, 10 Feb 2026 17:26:28 -0500 Subject: [PATCH 26/61] modify the names of the constants to include k at the start to remain consistent. --- src/main/java/frc/robot/Constants.java | 6 +++--- src/main/java/frc/robot/subsystems/guts/Guts.java | 2 +- src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a2af145..95fb1e5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -931,12 +931,12 @@ public class OperatorConstants { public static final class GutsConstants { - public static final int gutMotorID = 0; + public static final int kGutMotorID = 0; - public static final double gutMotorSpeed = 0.5; + public static final double kGutMotorSpeed = 0.5; //Change Gear Ratio later - public static final double gutMotorGearRatio = 0.0; + public static final double kGutMotorGearRatio = 0.0; } } diff --git a/src/main/java/frc/robot/subsystems/guts/Guts.java b/src/main/java/frc/robot/subsystems/guts/Guts.java index 2a3b0d0..e1991b0 100644 --- a/src/main/java/frc/robot/subsystems/guts/Guts.java +++ b/src/main/java/frc/robot/subsystems/guts/Guts.java @@ -22,7 +22,7 @@ public class Guts extends SubsystemBase { private final GutSide side; public final GutsIO io; public GutsIOInputsAutoLogged inputs = new GutsIOInputsAutoLogged(); - public double speed = (side == GutSide.LEFT) ? (GutsConstants.gutMotorSpeed) : -(GutsConstants.gutMotorSpeed); + public double speed = (side == GutSide.LEFT) ? (GutsConstants.kGutMotorSpeed) : -(GutsConstants.kGutMotorSpeed); /** Creates a new Guts. */ public Guts(GutSide side, GutsIO io) { diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java b/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java index ea91087..831487b 100644 --- a/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java @@ -19,7 +19,7 @@ */ public class GutsIOSparkMax implements GutsIO { - private final SparkMax GutMotor = new SparkMax(GutsConstants.gutMotorID, MotorType.kBrushless); + private final SparkMax GutMotor = new SparkMax(GutsConstants.kGutMotorID, MotorType.kBrushless); private final RelativeEncoder GutEncoder = GutMotor.getEncoder(); private final SparkMaxConfig GutMotorConfig; From a6b5439dca63fc80380ea3a11bfbc0d75b976f14 Mon Sep 17 00:00:00 2001 From: Ryan Hefferon Date: Tue, 10 Feb 2026 18:58:10 -0500 Subject: [PATCH 27/61] add the simulation IO layer and change the inputs to the units of radians and radians per second for position and velocity respectively. --- .../frc/robot/subsystems/guts/GutsIO.java | 6 ++- .../frc/robot/subsystems/guts/GutsIOSim.java | 44 ++++++++++++++++++- .../robot/subsystems/guts/GutsIOSparkMax.java | 7 ++- 3 files changed, 52 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIO.java b/src/main/java/frc/robot/subsystems/guts/GutsIO.java index c7aaca4..6406750 100644 --- a/src/main/java/frc/robot/subsystems/guts/GutsIO.java +++ b/src/main/java/frc/robot/subsystems/guts/GutsIO.java @@ -16,8 +16,10 @@ default void updateInputs(GutsIOInputs inputs) { /** Contains all the inputs regarding motors to be stored as data. */ @AutoLog public static class GutsIOInputs { - public double GutMotorVelocityRPM = 0.0; - public double GutMotorPositionRot = 0.0; + public double velocityRadPerSec = 0.0; + public double positionRad = 0.0; + public double appliedVolts = 0.0; + public double currentDrawAmps = 0.0; } /** Sets the gut motor to a specific speed ranging from -1.0 to 1.0 */ diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIOSim.java b/src/main/java/frc/robot/subsystems/guts/GutsIOSim.java index eb6cb5b..c3f75df 100644 --- a/src/main/java/frc/robot/subsystems/guts/GutsIOSim.java +++ b/src/main/java/frc/robot/subsystems/guts/GutsIOSim.java @@ -1,3 +1,45 @@ package frc.robot.subsystems.guts; -public class GutsIOSim implements GutsIO {} +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants; +import frc.robot.Constants.GutsConstants; + +public class GutsIOSim implements GutsIO { +private final DCMotor gearbox = DCMotor.getNEO(1); +private final DCMotorSim sim; + +//private final PIDController pid = new PIDController(1, 0, 0, Constants.kLoopPeriodSeconds); + +private double appliedVolts = 0.0; + +public GutsIOSim() { + sim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(gearbox, 0.025, GutsConstants.kGutMotorGearRatio), + gearbox + ); +} + +@Override +public void updateInputs(GutsIOInputs inputs) { + + appliedVolts = MathUtil.clamp(appliedVolts, -12.0, 12.0); + + sim.setInputVoltage(appliedVolts); + sim.update(0.02); + + inputs.positionRad = sim.getAngularPositionRotations(); + inputs.velocityRadPerSec = sim.getAngularVelocityRPM(); +} + +@Override +public void setGutMotorSpeed(double speed) { + appliedVolts = 12 * speed; +} + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java b/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java index 831487b..81c880f 100644 --- a/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java @@ -7,6 +7,7 @@ import com.revrobotics.PersistMode; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.util.Units; import frc.robot.Constants.GutsConstants; /** @@ -36,8 +37,10 @@ public void setGutMotorSpeed(double speed) { @Override public void updateInputs(GutsIOInputs inputs) { - inputs.GutMotorPositionRot = GutEncoder.getPosition(); - inputs.GutMotorVelocityRPM = GutEncoder.getVelocity(); + inputs.velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(GutEncoder.getVelocity()); + inputs.positionRad = Units.rotationsToRadians(GutEncoder.getPosition()); + inputs.appliedVolts = GutMotor.getAppliedOutput(); + inputs.currentDrawAmps = GutMotor.getOutputCurrent(); } } From fefe937910cec6a5014dfd055df86c19bf47cfc9 Mon Sep 17 00:00:00 2001 From: Ryan Hefferon Date: Tue, 10 Feb 2026 19:37:44 -0500 Subject: [PATCH 28/61] add the simulation IO layer, modify the inputs of both the arm and the roller to radians per second and radians for velocity and position respectively, and add the inputs of applied voltage and current amps. --- .../frc/robot/subsystems/intake/IntakeIO.java | 12 ++-- .../subsystems/intake/IntakeIOHardware.java | 13 ++-- .../robot/subsystems/intake/IntakeIOSim.java | 63 ++++++++++++++++++- 3 files changed, 79 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 12387e6..e0d7c24 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -18,10 +18,14 @@ default void updateInputs(IntakeIOInputs inputs) { @AutoLog public static class IntakeIOInputs { - double armMotorVelocityRPM = 0.0; - double wheelMotorVelocityRPM = 0.0; - double armMotorPositionsRotations = 0.0; - double wheelMotorPositionRotations = 0.0; + public double armVelocityRadPerSec = 0.0; + public double wheelVelocityRadPerSec = 0.0; + public double armPositionRad = 0.0; + public double wheelPositionRad = 0.0; + public double armAppliedVolts = 0.0; + public double wheelAppliedVolts = 0.0; + public double armCurrentDrawAmps = 0.0; + public double wheelCurrentDrawAmps = 0.0; } /** diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java index 89903e3..7141e07 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java @@ -6,6 +6,7 @@ import com.revrobotics.spark.config.EncoderConfig; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DigitalInput; import frc.robot.Constants.IntakeConstants; @@ -36,10 +37,14 @@ public void setWheelSpeed(double speed) { @Override public void updateInputs(IntakeIOInputs inputs){ - inputs.armMotorVelocityRPM = armEncoder.getVelocity(); - inputs.wheelMotorVelocityRPM = wheelEncoder.getVelocity(); - inputs.armMotorPositionsRotations = armEncoder.getPosition(); - inputs.wheelMotorPositionRotations = wheelEncoder.getPosition(); + inputs.armVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(armEncoder.getVelocity()); + inputs.wheelVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(wheelEncoder.getVelocity()); + inputs.armPositionRad = Units.rotationsToRadians(armEncoder.getPosition()); + inputs.wheelPositionRad = Units.rotationsToRadians(wheelEncoder.getPosition()); + inputs.armAppliedVolts = armMotor.getAppliedOutput(); + inputs.wheelAppliedVolts = wheelMotor.getAppliedOutput(); + inputs.armCurrentDrawAmps = armMotor.getOutputCurrent(); + inputs.wheelCurrentDrawAmps = wheelMotor.getOutputCurrent(); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 6c412da..9a72d64 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -1,3 +1,64 @@ package frc.robot.subsystems.intake; -public class IntakeIOSim implements IntakeIO {} +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants.IntakeConstants; + +public class IntakeIOSim implements IntakeIO { + +private final DCMotor gearbox = DCMotor.getNEO(2); +private final DCMotorSim armSim; +private final DCMotorSim wheelSim; + +//private final PIDController pid = new PIDController(1, 0, 0, Constants.kLoopPeriodSeconds); + +private double armAppliedVolts = 0.0; +private double wheelAppliedVolts = 0.0; + +public IntakeIOSim() { + armSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(gearbox, 0.025, IntakeConstants.kPivotMotorGearRatio), + gearbox + ); + + wheelSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(gearbox, 0.025, IntakeConstants.kRollerMotorGearRatio), + gearbox + ); +} + +@Override +public void updateInputs(IntakeIOInputs inputs) { + + armAppliedVolts = MathUtil.clamp(armAppliedVolts, -12.0, 12.0); + wheelAppliedVolts = MathUtil.clamp(wheelAppliedVolts, -12.0, 12.0); + + armSim.setInputVoltage(armAppliedVolts); + armSim.update(0.02); + + wheelSim.setInputVoltage(wheelAppliedVolts); + wheelSim.update(0.02); + + inputs.armPositionRad = armSim.getAngularPositionRotations(); + inputs.armVelocityRadPerSec = armSim.getAngularVelocityRPM(); + + inputs.wheelPositionRad = wheelSim.getAngularPositionRotations(); + inputs.wheelVelocityRadPerSec = wheelSim.getAngularVelocityRPM(); +} + +@Override +public void setArmSpeed(double speed) { + armAppliedVolts = 12 * speed; +} + +@Override +public void setWheelSpeed(double speed) { + wheelAppliedVolts = 12 * speed; +} + +} From 0823d379fcb0006cf074192196969f2b68c424f9 Mon Sep 17 00:00:00 2001 From: Ryan Hefferon Date: Tue, 10 Feb 2026 19:39:50 -0500 Subject: [PATCH 29/61] fix the names of the objects to follow camelCase --- .../robot/subsystems/guts/GutsIOSparkMax.java | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java b/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java index 81c880f..e29fc2c 100644 --- a/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java @@ -20,27 +20,27 @@ */ public class GutsIOSparkMax implements GutsIO { - private final SparkMax GutMotor = new SparkMax(GutsConstants.kGutMotorID, MotorType.kBrushless); - private final RelativeEncoder GutEncoder = GutMotor.getEncoder(); - private final SparkMaxConfig GutMotorConfig; + private final SparkMax gutMotor = new SparkMax(GutsConstants.kGutMotorID, MotorType.kBrushless); + private final RelativeEncoder gutEncoder = gutMotor.getEncoder(); + private final SparkMaxConfig gutMotorConfig; public GutsIOSparkMax() { - GutMotorConfig = new SparkMaxConfig(); + gutMotorConfig = new SparkMaxConfig(); - GutMotor.configure(GutMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + gutMotor.configure(gutMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); } @Override public void setGutMotorSpeed(double speed) { - GutMotor.set(speed); + gutMotor.set(speed); } @Override public void updateInputs(GutsIOInputs inputs) { - inputs.velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(GutEncoder.getVelocity()); - inputs.positionRad = Units.rotationsToRadians(GutEncoder.getPosition()); - inputs.appliedVolts = GutMotor.getAppliedOutput(); - inputs.currentDrawAmps = GutMotor.getOutputCurrent(); + inputs.velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(gutEncoder.getVelocity()); + inputs.positionRad = Units.rotationsToRadians(gutEncoder.getPosition()); + inputs.appliedVolts = gutMotor.getAppliedOutput(); + inputs.currentDrawAmps = gutMotor.getOutputCurrent(); } } From 110e9cf3905dd8db44520aa64082bb386814c1d3 Mon Sep 17 00:00:00 2001 From: Matthew McGrath Date: Wed, 11 Feb 2026 19:35:44 -0500 Subject: [PATCH 30/61] Added buttons and fixed motor type for the pivot and added commands to run motors backwards --- src/main/java/frc/robot/Constants.java | 10 ++++ src/main/java/frc/robot/RobotContainer.java | 14 ++++-- .../frc/robot/subsystems/intake/Intake.java | 38 +++++++++++---- .../frc/robot/subsystems/intake/IntakeIO.java | 14 +++--- .../subsystems/intake/IntakeIOHardware.java | 47 ++++++++++--------- .../robot/subsystems/intake/IntakeIOSim.java | 31 ++++++------ 6 files changed, 99 insertions(+), 55 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e21d25a..4ea959d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -53,7 +53,10 @@ import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.MomentOfInertia; import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + import java.util.Map; /** @@ -927,4 +930,11 @@ public static class IntakeConstants { public static final double kPivotMotorGearRatio = 0.0; public static final double kRollerMotorGearRatio = 0.0; } + public static class OperatorConstants{ + public final static Joystick auxStick = new Joystick(7); + public static JoystickButton kIntakeButton1 = new JoystickButton(auxStick, 4); + public static JoystickButton kIntakeButton2 = new JoystickButton(auxStick, 5); + public static JoystickButton kIntakeButton3 = new JoystickButton(auxStick, 6); + public static JoystickButton kIntakeButton4 = new JoystickButton(auxStick, 7); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f1b07c5..e6daba6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,6 +20,8 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.IntakeIO; import frc.robot.subsystems.vision.CameraIO; import frc.robot.subsystems.vision.Vision; import frc.robot.util.AllianceFlipUtil; @@ -29,8 +31,9 @@ public class RobotContainer { private final CommandXboxController driver = new CommandXboxController(0); - private final Drive drive; - private final Vision vision; + private Drive drive; + private Vision vision; + private Intake intake; public RobotContainer() { switch (Constants.kCurrentMode) { @@ -65,7 +68,7 @@ public RobotContainer() { new ModuleIO() {}); vision = new Vision(null, new CameraIO[] {}); break; - } + } configureBindings(); } @@ -100,6 +103,11 @@ private void configureBindings() { drive, () -> RobotState.getInstance().getEstimatedPose(), () -> Hub.innerCenterPoint.toTranslation2d())); + + Constants.OperatorConstants.kIntakeButton1.whileTrue(intake.runPivot()); + Constants.OperatorConstants.kIntakeButton2.whileTrue(intake.runFeeder()); + Constants.OperatorConstants.kIntakeButton3.whileTrue(intake.runPivotBack()); + Constants.OperatorConstants.kIntakeButton4.whileTrue(intake.runFeederBack()); } public void robotPeriodic() { diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index f407349..8daf81f 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.IntakeConstants; public class Intake extends SubsystemBase { /** Creates a new Intake. */ @@ -22,24 +23,43 @@ public Intake(IntakeIO io) { this.io = io; } /** - * Command to run the arm - * @param speed runs the arm at a set speed - * @return runs the arm at a speed on every iteration until end when it stops the running + * Command to run the pivot + * @return runs the pivot at a speed on every iteration until end when it stops the running */ - public Command runArm(double speed) { + public Command runPivot() { return Commands.runEnd( - () -> io.setArmSpeed(speed), - () -> io.setArmSpeed(0.0), + () -> io.setPivotSpeed(IntakeConstants.kPivotMotorSpeed), + () -> io.setPivotSpeed(0.0), + this); + } +/** + * Command to run the pivot back + * @return runs the pivot at a speed on every iteration until end when it stops the running + */ + public Command runPivotBack() { + return Commands.runEnd( + () -> io.setPivotSpeed(-(IntakeConstants.kPivotMotorSpeed)), + () -> io.setPivotSpeed(0.0), this); } /** * Command to run the feeder - * @param speed runs the feeder at a set speed * @return runs the feeder at a speed on every iteration until end when it stops the running */ - public Command runFeeder(double speed) { + public Command runFeeder() { + return Commands.runEnd( + () -> io.setWheelSpeed(IntakeConstants.kRollerMotorSpeed), + () -> io.setWheelSpeed(0.0), + this); + } + +/** + * Command to run the feeder backward + * @return runs the feeder at a speed on every iteration until end when it stops the running + */ + public Command runFeederBack() { return Commands.runEnd( - () -> io.setWheelSpeed(speed), + () -> io.setWheelSpeed(-(IntakeConstants.kRollerMotorSpeed)), () -> io.setWheelSpeed(0.0), this); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index e0d7c24..c129e7e 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -18,21 +18,21 @@ default void updateInputs(IntakeIOInputs inputs) { @AutoLog public static class IntakeIOInputs { - public double armVelocityRadPerSec = 0.0; + public double pivotVelocityRadPerSec = 0.0; public double wheelVelocityRadPerSec = 0.0; - public double armPositionRad = 0.0; + public double pivotPositionRad = 0.0; public double wheelPositionRad = 0.0; - public double armAppliedVolts = 0.0; + public double pivotAppliedVolts = 0.0; public double wheelAppliedVolts = 0.0; - public double armCurrentDrawAmps = 0.0; + public double pivotCurrentDrawAmps = 0.0; public double wheelCurrentDrawAmps = 0.0; } /** - * method to set the speed of the arm - * @param speed determines the speed of the arm on a scale of -1 to 1 + * method to set the speed of the pivot + * @param speed determines the speed of the pivot on a scale of -1 to 1 */ - default void setArmSpeed(double speed){} + default void setPivotSpeed(double speed){} /** * method to set the speed of the wheel * @param speed determines the speed of the wheel on a scale of -1 to 1 diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java index 7141e07..99730e5 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java @@ -1,7 +1,15 @@ package frc.robot.subsystems.intake; +import java.lang.module.Configuration; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.hardware.TalonFX; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkMax; +import com.revrobotics.ResetMode; +import com.revrobotics.PersistMode; +import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.EncoderConfig; import com.revrobotics.spark.config.SparkMaxConfig; @@ -11,23 +19,20 @@ import frc.robot.Constants.IntakeConstants; public class IntakeIOHardware implements IntakeIO { - SparkMax armMotor = new SparkMax(IntakeConstants.kPivotMotorID, MotorType.kBrushless); - SparkMax wheelMotor = new SparkMax(IntakeConstants.kRollerMotorID, MotorType.kBrushless); - RelativeEncoder armEncoder = armMotor.getEncoder(); - RelativeEncoder wheelEncoder = wheelMotor.getEncoder(); - SparkMaxConfig armConfig; - SparkMaxConfig wheelConfig; - + private SparkMax pivotMotor = new SparkMax(IntakeConstants.kPivotMotorID, MotorType.kBrushless); + private RelativeEncoder pivotEncoder = pivotMotor.getEncoder(); + private TalonFX wheelMotor = new TalonFX(IntakeConstants.kRollerMotorID); + private SparkMaxConfig pivotConfig; + private TalonFXConfiguration wheelMotorConfig; public IntakeIOHardware() { - armConfig = new SparkMaxConfig(); - wheelConfig = new SparkMaxConfig(); - // armMotor.configure(armConfig, null, null); - // wheelMotor.configure(armConfig, null, null); + pivotConfig = new SparkMaxConfig(); + wheelMotor.getConfigurator().apply(wheelMotorConfig); + pivotMotor.configure(pivotConfig, ResetMode.kNoResetSafeParameters, null); } @Override - public void setArmSpeed(double speed) { - armMotor.set(speed); + public void setPivotSpeed(double speed) { + pivotMotor.set(speed); } @Override @@ -37,14 +42,14 @@ public void setWheelSpeed(double speed) { @Override public void updateInputs(IntakeIOInputs inputs){ - inputs.armVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(armEncoder.getVelocity()); - inputs.wheelVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(wheelEncoder.getVelocity()); - inputs.armPositionRad = Units.rotationsToRadians(armEncoder.getPosition()); - inputs.wheelPositionRad = Units.rotationsToRadians(wheelEncoder.getPosition()); - inputs.armAppliedVolts = armMotor.getAppliedOutput(); - inputs.wheelAppliedVolts = wheelMotor.getAppliedOutput(); - inputs.armCurrentDrawAmps = armMotor.getOutputCurrent(); - inputs.wheelCurrentDrawAmps = wheelMotor.getOutputCurrent(); + inputs.pivotVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(pivotEncoder.getVelocity()); + inputs.wheelVelocityRadPerSec = Units.rotationsToRadians(wheelMotor.getVelocity().getValueAsDouble()); + inputs.pivotPositionRad = Units.rotationsToRadians(pivotEncoder.getPosition()); + inputs.wheelPositionRad = Units.rotationsToRadians(wheelMotor.getPosition().getValueAsDouble()); + inputs.pivotAppliedVolts = pivotMotor.getAppliedOutput(); + inputs.wheelAppliedVolts = wheelMotor.getTorqueCurrent().getValueAsDouble(); + inputs.pivotCurrentDrawAmps = pivotMotor.getOutputCurrent(); + inputs.wheelCurrentDrawAmps = wheelMotor.getMotorVoltage().getValueAsDouble(); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 9a72d64..c4ae8e3 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -9,51 +9,52 @@ public class IntakeIOSim implements IntakeIO { -private final DCMotor gearbox = DCMotor.getNEO(2); -private final DCMotorSim armSim; +private final DCMotor pivotGearbox = DCMotor.getNEO(1); +private final DCMotor wheelGearbox = DCMotor.getKrakenX60(1); +private final DCMotorSim pivotSim; private final DCMotorSim wheelSim; //private final PIDController pid = new PIDController(1, 0, 0, Constants.kLoopPeriodSeconds); -private double armAppliedVolts = 0.0; +private double pivotAppliedVolts = 0.0; private double wheelAppliedVolts = 0.0; public IntakeIOSim() { - armSim = + pivotSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(gearbox, 0.025, IntakeConstants.kPivotMotorGearRatio), - gearbox + LinearSystemId.createDCMotorSystem(pivotGearbox, 0.025, IntakeConstants.kPivotMotorGearRatio), + pivotGearbox ); wheelSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(gearbox, 0.025, IntakeConstants.kRollerMotorGearRatio), - gearbox + LinearSystemId.createDCMotorSystem(wheelGearbox, 0.025, IntakeConstants.kRollerMotorGearRatio), + wheelGearbox ); } @Override public void updateInputs(IntakeIOInputs inputs) { - armAppliedVolts = MathUtil.clamp(armAppliedVolts, -12.0, 12.0); + pivotAppliedVolts = MathUtil.clamp(pivotAppliedVolts, -12.0, 12.0); wheelAppliedVolts = MathUtil.clamp(wheelAppliedVolts, -12.0, 12.0); - armSim.setInputVoltage(armAppliedVolts); - armSim.update(0.02); + pivotSim.setInputVoltage(pivotAppliedVolts); + pivotSim.update(0.02); wheelSim.setInputVoltage(wheelAppliedVolts); wheelSim.update(0.02); - inputs.armPositionRad = armSim.getAngularPositionRotations(); - inputs.armVelocityRadPerSec = armSim.getAngularVelocityRPM(); + inputs.pivotPositionRad = pivotSim.getAngularPositionRotations(); + inputs.pivotVelocityRadPerSec = pivotSim.getAngularVelocityRPM(); inputs.wheelPositionRad = wheelSim.getAngularPositionRotations(); inputs.wheelVelocityRadPerSec = wheelSim.getAngularVelocityRPM(); } @Override -public void setArmSpeed(double speed) { - armAppliedVolts = 12 * speed; +public void setPivotSpeed(double speed) { + pivotAppliedVolts = 12 * speed; } @Override From e002bbc270329ab9117a7bbf93dff630c6fd3fb1 Mon Sep 17 00:00:00 2001 From: Matthew McGrath Date: Wed, 11 Feb 2026 19:46:17 -0500 Subject: [PATCH 31/61] formated code better --- src/main/java/frc/robot/Constants.java | 12 ++-- src/main/java/frc/robot/RobotContainer.java | 3 +- .../frc/robot/subsystems/intake/Intake.java | 48 +++++++------- .../frc/robot/subsystems/intake/IntakeIO.java | 15 ++--- .../subsystems/intake/IntakeIOHardware.java | 66 +++++++++---------- .../robot/subsystems/intake/IntakeIOSim.java | 56 ++++++++-------- 6 files changed, 95 insertions(+), 105 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4ea959d..469fb72 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -56,7 +56,6 @@ import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.button.JoystickButton; - import java.util.Map; /** @@ -926,12 +925,13 @@ public static class IntakeConstants { public static final double kPivotMotorSpeed = 0.5; public static final double kRollerMotorSpeed = 0.5; - //Change Gear Ratios later - public static final double kPivotMotorGearRatio = 0.0; - public static final double kRollerMotorGearRatio = 0.0; + // Change Gear Ratios later + public static final double kPivotMotorGearRatio = 1.0; + public static final double kRollerMotorGearRatio = 1.0; } - public static class OperatorConstants{ - public final static Joystick auxStick = new Joystick(7); + + public static class OperatorConstants { + public static final Joystick auxStick = new Joystick(7); public static JoystickButton kIntakeButton1 = new JoystickButton(auxStick, 4); public static JoystickButton kIntakeButton2 = new JoystickButton(auxStick, 5); public static JoystickButton kIntakeButton3 = new JoystickButton(auxStick, 6); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e6daba6..8e699a0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,7 +21,6 @@ import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.intake.IntakeIO; import frc.robot.subsystems.vision.CameraIO; import frc.robot.subsystems.vision.Vision; import frc.robot.util.AllianceFlipUtil; @@ -68,7 +67,7 @@ public RobotContainer() { new ModuleIO() {}); vision = new Vision(null, new CameraIO[] {}); break; - } + } configureBindings(); } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 8daf81f..ad9b543 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -4,48 +4,48 @@ package frc.robot.subsystems.intake; -import org.littletonrobotics.junction.Logger; - -import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.IntakeConstants; +import org.littletonrobotics.junction.Logger; public class Intake extends SubsystemBase { /** Creates a new Intake. */ - private final IntakeIO io; + private final IntakeIOAutoLogged inputs = new IntakeIOAutoLogged(); public Intake(IntakeIO io) { this.io = io; } -/** - * Command to run the pivot - * @return runs the pivot at a speed on every iteration until end when it stops the running - */ + /** + * Command to run the pivot + * + * @return runs the pivot at a speed on every iteration until end when it stops the running + */ public Command runPivot() { return Commands.runEnd( () -> io.setPivotSpeed(IntakeConstants.kPivotMotorSpeed), () -> io.setPivotSpeed(0.0), this); } -/** - * Command to run the pivot back - * @return runs the pivot at a speed on every iteration until end when it stops the running - */ + /** + * Command to run the pivot back + * + * @return runs the pivot at a speed on every iteration until end when it stops the running + */ public Command runPivotBack() { return Commands.runEnd( () -> io.setPivotSpeed(-(IntakeConstants.kPivotMotorSpeed)), () -> io.setPivotSpeed(0.0), this); } -/** - * Command to run the feeder - * @return runs the feeder at a speed on every iteration until end when it stops the running - */ + /** + * Command to run the feeder + * + * @return runs the feeder at a speed on every iteration until end when it stops the running + */ public Command runFeeder() { return Commands.runEnd( () -> io.setWheelSpeed(IntakeConstants.kRollerMotorSpeed), @@ -53,18 +53,18 @@ public Command runFeeder() { this); } -/** - * Command to run the feeder backward - * @return runs the feeder at a speed on every iteration until end when it stops the running - */ - public Command runFeederBack() { + /** + * Command to run the feeder backward + * + * @return runs the feeder at a speed on every iteration until end when it stops the running + */ + public Command runFeederBack() { return Commands.runEnd( () -> io.setWheelSpeed(-(IntakeConstants.kRollerMotorSpeed)), () -> io.setWheelSpeed(0.0), this); } -//potential sequences for commands in future - + // potential sequences for commands in future // public Command extendArmSequence() { // return Commands.run(() -> runArm(.5), this) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index c129e7e..fd0a2cc 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -3,8 +3,7 @@ import org.littletonrobotics.junction.AutoLog; /** - * The {@code IntakeIO} class provides methods for interacting with the intake - * motors and updating + * The {@code IntakeIO} class provides methods for interacting with the intake motors and updating * the intake inputs. * * @author Ryan Hefferon @@ -13,8 +12,7 @@ * @author Julien Precourt */ public interface IntakeIO { - default void updateInputs(IntakeIOInputs inputs) { - } + default void updateInputs(IntakeIOInputs inputs) {} @AutoLog public static class IntakeIOInputs { @@ -26,16 +24,17 @@ public static class IntakeIOInputs { public double wheelAppliedVolts = 0.0; public double pivotCurrentDrawAmps = 0.0; public double wheelCurrentDrawAmps = 0.0; - } /** * method to set the speed of the pivot + * * @param speed determines the speed of the pivot on a scale of -1 to 1 */ - default void setPivotSpeed(double speed){} - /** + default void setPivotSpeed(double speed) {} + /** * method to set the speed of the wheel + * * @param speed determines the speed of the wheel on a scale of -1 to 1 */ - default void setWheelSpeed(double speed){} + default void setWheelSpeed(double speed) {} } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java index 99730e5..ff48655 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java @@ -1,55 +1,49 @@ package frc.robot.subsystems.intake; -import java.lang.module.Configuration; - import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.hardware.TalonFX; import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkMax; import com.revrobotics.ResetMode; -import com.revrobotics.PersistMode; -import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkMaxConfig; - import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DigitalInput; import frc.robot.Constants.IntakeConstants; public class IntakeIOHardware implements IntakeIO { - private SparkMax pivotMotor = new SparkMax(IntakeConstants.kPivotMotorID, MotorType.kBrushless); - private RelativeEncoder pivotEncoder = pivotMotor.getEncoder(); - private TalonFX wheelMotor = new TalonFX(IntakeConstants.kRollerMotorID); - private SparkMaxConfig pivotConfig; - private TalonFXConfiguration wheelMotorConfig; - public IntakeIOHardware() { - pivotConfig = new SparkMaxConfig(); - wheelMotor.getConfigurator().apply(wheelMotorConfig); - pivotMotor.configure(pivotConfig, ResetMode.kNoResetSafeParameters, null); - } - - @Override - public void setPivotSpeed(double speed) { - pivotMotor.set(speed); - } - - @Override - public void setWheelSpeed(double speed) { - wheelMotor.set(speed); - } - - @Override - public void updateInputs(IntakeIOInputs inputs){ - inputs.pivotVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(pivotEncoder.getVelocity()); - inputs.wheelVelocityRadPerSec = Units.rotationsToRadians(wheelMotor.getVelocity().getValueAsDouble()); + private SparkMax pivotMotor = new SparkMax(IntakeConstants.kPivotMotorID, MotorType.kBrushless); + private RelativeEncoder pivotEncoder = pivotMotor.getEncoder(); + private TalonFX wheelMotor = new TalonFX(IntakeConstants.kRollerMotorID); + private SparkMaxConfig pivotConfig; + private TalonFXConfiguration wheelMotorConfig; + + public IntakeIOHardware() { + pivotConfig = new SparkMaxConfig(); + wheelMotor.getConfigurator().apply(wheelMotorConfig); + pivotMotor.configure(pivotConfig, ResetMode.kNoResetSafeParameters, null); + } + + @Override + public void setPivotSpeed(double speed) { + pivotMotor.set(speed); + } + + @Override + public void setWheelSpeed(double speed) { + wheelMotor.set(speed); + } + + @Override + public void updateInputs(IntakeIOInputs inputs) { + inputs.pivotVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(pivotEncoder.getVelocity()); + inputs.wheelVelocityRadPerSec = + Units.rotationsToRadians(wheelMotor.getVelocity().getValueAsDouble()); inputs.pivotPositionRad = Units.rotationsToRadians(pivotEncoder.getPosition()); inputs.wheelPositionRad = Units.rotationsToRadians(wheelMotor.getPosition().getValueAsDouble()); inputs.pivotAppliedVolts = pivotMotor.getAppliedOutput(); inputs.wheelAppliedVolts = wheelMotor.getTorqueCurrent().getValueAsDouble(); inputs.pivotCurrentDrawAmps = pivotMotor.getOutputCurrent(); inputs.wheelCurrentDrawAmps = wheelMotor.getMotorVoltage().getValueAsDouble(); - } - + } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index c4ae8e3..8124a74 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.intake; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.simulation.DCMotorSim; @@ -9,32 +8,32 @@ public class IntakeIOSim implements IntakeIO { -private final DCMotor pivotGearbox = DCMotor.getNEO(1); -private final DCMotor wheelGearbox = DCMotor.getKrakenX60(1); -private final DCMotorSim pivotSim; -private final DCMotorSim wheelSim; + private final DCMotor pivotGearbox = DCMotor.getNEO(1); + private final DCMotor wheelGearbox = DCMotor.getKrakenX60(1); + private final DCMotorSim pivotSim; + private final DCMotorSim wheelSim; -//private final PIDController pid = new PIDController(1, 0, 0, Constants.kLoopPeriodSeconds); + // private final PIDController pid = new PIDController(1, 0, 0, Constants.kLoopPeriodSeconds); -private double pivotAppliedVolts = 0.0; -private double wheelAppliedVolts = 0.0; + private double pivotAppliedVolts = 0.0; + private double wheelAppliedVolts = 0.0; -public IntakeIOSim() { - pivotSim = + public IntakeIOSim() { + pivotSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(pivotGearbox, 0.025, IntakeConstants.kPivotMotorGearRatio), - pivotGearbox - ); + LinearSystemId.createDCMotorSystem( + pivotGearbox, 0.025, IntakeConstants.kPivotMotorGearRatio), + pivotGearbox); - wheelSim = + wheelSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(wheelGearbox, 0.025, IntakeConstants.kRollerMotorGearRatio), - wheelGearbox - ); -} + LinearSystemId.createDCMotorSystem( + wheelGearbox, 0.025, IntakeConstants.kRollerMotorGearRatio), + wheelGearbox); + } -@Override -public void updateInputs(IntakeIOInputs inputs) { + @Override + public void updateInputs(IntakeIOInputs inputs) { pivotAppliedVolts = MathUtil.clamp(pivotAppliedVolts, -12.0, 12.0); wheelAppliedVolts = MathUtil.clamp(wheelAppliedVolts, -12.0, 12.0); @@ -47,19 +46,18 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.pivotPositionRad = pivotSim.getAngularPositionRotations(); inputs.pivotVelocityRadPerSec = pivotSim.getAngularVelocityRPM(); - + inputs.wheelPositionRad = wheelSim.getAngularPositionRotations(); inputs.wheelVelocityRadPerSec = wheelSim.getAngularVelocityRPM(); -} + } -@Override -public void setPivotSpeed(double speed) { + @Override + public void setPivotSpeed(double speed) { pivotAppliedVolts = 12 * speed; -} + } -@Override -public void setWheelSpeed(double speed) { + @Override + public void setWheelSpeed(double speed) { wheelAppliedVolts = 12 * speed; -} - + } } From abe02f545fb02a911740d878e825839c86da812e Mon Sep 17 00:00:00 2001 From: Matthew McGrath Date: Thu, 12 Feb 2026 18:21:45 -0500 Subject: [PATCH 32/61] Added noresistparamters to the motor and added formatting --- src/main/java/frc/robot/Constants.java | 1652 ++++++++--------- src/main/java/frc/robot/RobotContainer.java | 53 +- .../frc/robot/subsystems/intake/Intake.java | 33 +- .../frc/robot/subsystems/intake/IntakeIO.java | 14 +- .../subsystems/intake/IntakeIOHardware.java | 9 +- 5 files changed, 885 insertions(+), 876 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 469fb72..962951d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -59,619 +59,608 @@ import java.util.Map; /** - * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running - * on a roboRIO. Change the value of "simMode" to switch between "sim" (physics sim) and "replay" + * This class defines the runtime mode used by AdvantageKit. The mode is always + * "real" when running + * on a roboRIO. Change the value of "simMode" to switch between "sim" (physics + * sim) and "replay" * (log replay from a file). */ public final class Constants { - public static final double kLoopPeriodSeconds = 0.02; + public static final double kLoopPeriodSeconds = 0.02; - public static final Mode kSimMode = Mode.SIM; - public static final Mode kCurrentMode = RobotBase.isReal() ? Mode.REAL : kSimMode; + public static final Mode kSimMode = Mode.SIM; + public static final Mode kCurrentMode = RobotBase.isReal() ? Mode.REAL : kSimMode; - public static enum Mode { - /** Running on a real robot. */ - REAL, + public static enum Mode { + /** Running on a real robot. */ + REAL, - /** Running a physics simulator. */ - SIM, + /** Running a physics simulator. */ + SIM, - /** Replaying from a log file. */ - REPLAY - } + /** Replaying from a log file. */ + REPLAY + } - public static boolean kDisableHAL = false; + public static boolean kDisableHAL = false; - public static void disableHAL() { - kDisableHAL = true; - } + public static void disableHAL() { + kDisableHAL = true; + } - public static final class DriveConstants { + public static final class DriveConstants { - public static final class ModuleConfigs { + public static final class ModuleConfigs { - public static record ModuleConfig( - int driveMotorID, int angleMotorID, int canCoderID, Rotation2d angleOffset) {} + public static record ModuleConfig( + int driveMotorID, int angleMotorID, int canCoderID, Rotation2d angleOffset) { + } - /** Module 0 (front left) configs. */ - public static final ModuleConfig FrontLeft = - new ModuleConfig(1, 2, 19, Rotation2d.fromDegrees(304.36523 - 180)); + /** Module 0 (front left) configs. */ + public static final ModuleConfig FrontLeft = new ModuleConfig(1, 2, 19, + Rotation2d.fromDegrees(304.36523 - 180)); - /** Module 1 (front right) configs. */ - public static final ModuleConfig FrontRight = - new ModuleConfig(2, 4, 20, Rotation2d.fromDegrees(206.455)); + /** Module 1 (front right) configs. */ + public static final ModuleConfig FrontRight = new ModuleConfig(2, 4, 20, Rotation2d.fromDegrees(206.455)); - /** Module 2 (back left) configs. */ - public static final ModuleConfig BackLeft = - new ModuleConfig(5, 6, 21, Rotation2d.fromDegrees(35.419922 + 180)); + /** Module 2 (back left) configs. */ + public static final ModuleConfig BackLeft = new ModuleConfig(5, 6, 21, + Rotation2d.fromDegrees(35.419922 + 180)); - /** Module 3 (back right) configs. */ - public static final ModuleConfig BackRight = - new ModuleConfig(7, 8, 22, Rotation2d.fromDegrees(116.89453)); - } + /** Module 3 (back right) configs. */ + public static final ModuleConfig BackRight = new ModuleConfig(7, 8, 22, Rotation2d.fromDegrees(116.89453)); + } - // TunerConstants doesn't include these constants - public static final double kOdometryFrequency = - ModuleConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0; - public static final double kDriveBaseRadius = - Math.max( - Math.max( - Math.hypot( - ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), - Math.hypot( - ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY)), - Math.max( - Math.hypot(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), - Math.hypot( - ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY))); - - public static final Translation2d[] kModuleTranslations = - new Translation2d[] { - new Translation2d( - ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), - new Translation2d( - ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY), - new Translation2d(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), - new Translation2d( - ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY) + // TunerConstants doesn't include these constants + public static final double kOdometryFrequency = ModuleConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0; + public static final double kDriveBaseRadius = Math.max( + Math.max( + Math.hypot( + ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), + Math.hypot( + ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY)), + Math.max( + Math.hypot(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), + Math.hypot( + ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY))); + + public static final Translation2d[] kModuleTranslations = new Translation2d[] { + new Translation2d( + ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), + new Translation2d( + ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY), + new Translation2d(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), + new Translation2d( + ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY) }; - // PathPlanner config constants - public static final double kRobotMassKG = 74.088; - public static final double kRobotMOI = 6.883; - /** Coefficient of friction */ - public static final double kWheelCOF = 1.2; - - public static final RobotConfig kPathplannerConfig = - new RobotConfig( - kRobotMOI, - kRobotMOI, - new ModuleConfig( - ModuleConstants.FrontLeft.WheelRadius, - ModuleConstants.kSpeedAt12Volts.in(MetersPerSecond), - kWheelCOF, - DCMotor.getKrakenX60Foc(1) - .withReduction(ModuleConstants.FrontLeft.DriveMotorGearRatio), - ModuleConstants.FrontLeft.SlipCurrent, - 1), - kModuleTranslations); - - public static final IdleMode kDriveIdleMode = IdleMode.kBrake; - public static final IdleMode kAngleIdleMode = IdleMode.kBrake; - public static final double kDrivePower = 1; - public static final double kAnglePower = .9; - - public static final boolean kInvertGyro = false; // Always ensure Gyro is CCW+ CW- - - // drivetrain constants - public static final double kTrackWidth = Units.inchesToMeters(24.75); - public static final double kWheelBase = Units.inchesToMeters(24.75); - public static final double kWheelDiameter = Units.inchesToMeters(4.0); - public static final double kWheelRadius = kWheelDiameter / 2.0; - public static final double kWheelCircumference = kWheelDiameter * Math.PI; - - // Swerve kinematics, don't change - public static final SwerveDriveKinematics swerveKinematics = - new SwerveDriveKinematics( - new Translation2d(kWheelBase / 2.0, kTrackWidth / 2.0), // front left - new Translation2d(kWheelBase / 2.0, -kTrackWidth / 2.0), // front right - new Translation2d(-kWheelBase / 2.0, kTrackWidth / 2.0), // back left - new Translation2d(-kWheelBase / 2.0, -kTrackWidth / 2.0)); // back right - - // gear ratios - public static final double kDriveGearRatio = (6.12 / 1.0); - public static final double kAngleGearRatio = ((150.0 / 7.0) / 1.0); - - // encoder stuff - // meters per rotation - public static final double kDriveRevToMeters = kWheelCircumference / (kDriveGearRatio); - public static final double kDriveRpmToMetersPerSecond = kDriveRevToMeters / 60; - - /** The number of degrees that a single rotation of the turn motor turns the // wheel. */ - public static final double kDegreesPerTurnRotation = 360 / kAngleGearRatio; - - // motor inverts, check these - public static final boolean kAngleMotorInvert = true; - public static final InvertedValue kDriveMotorInvert = InvertedValue.CounterClockwise_Positive; - - /* Angle Encoder Invert */ - public static final boolean kCanCoderInvert = false; - - /* Swerve Current Limiting */ - public static final int kAngleContinuousCurrentLimit = 20; - public static final int kAnglePeakCurrentLimit = 40; - public static final double kAnglePeakCurrentDuration = 0.1; - public static final boolean kAngleEnableCurrentLimit = true; - - public static final int kDriveSupplyCurrentLimit = 60; - public static final boolean kDriveSupplyCurrentLimitEnable = true; - public static final int kDriveSupplyCurrentThreshold = 60; - public static final double kDriveSupplyTimeThreshold = 0.1; - - public static final boolean kDriveEnableCurrentLimit = true; - - /* - * These values are used by the drive falcon to ramp in open loop and closed - * loop driving. - * We found a small open loop ramp (0.25) helps with tread wear, tipping, etc - */ - public static final double kOpenLoopRamp = 0.25; - public static final double kClosedLoopRamp = 0.0; - - /* Angle Motor PID Values */ - public static final double kAngleKP = 0.015; - public static final double kAngleKI = 0; - public static final double kAngleKD = 0; - public static final double kAngleKF = 0; - - /* Drive Motor PID Values */ - - public static final double kDriveKP = 0.01; - public static final double kDriveKI = 0.0; - public static final double kDriveKD = 0.0; - - public static final double kDriveKS = (0.32 / 12); - public static final double kDriveKV = (1.988 / 12); - public static final double kDriveKA = (1.0449 / 12); - - /* Swerve Profiling Values */ - /** Meters per second. */ - public static final double kPhysicalMaxSpeed = 5.0; - - public static final double kMaxTeleDriveSpeed = 4.5; - /** Radians per second. */ - public static final double kPhysicalMaxAngularSpeed = 2 * 2 * Math.PI; - /** Radians per second. */ - public static final double kMaxTeleAngularSpeed = kPhysicalMaxAngularSpeed / 2; - - public static final double kMaxAngularAccelerationSpeed = 4 / Math.PI; - /** Radians per second. */ - public static final double kMaxTeleAngularAccelerationSpeed = kMaxAngularAccelerationSpeed / 2; - - public static final double kDeadband = 0.08; - - public static final Map kDistances = - Map.of( - 0, 0.0, - 1, 1.0, - 2, 2.0, - 3, 3.0, - 4, 4.0); - - public static class ModuleConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with - // the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = - new Slot0Configs() - .withKP(100) - .withKI(0) - .withKD(0.5) - .withKS(0.1) - .withKV(1.91) - .withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = - new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = - ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = - ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = - DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = - SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to - // RemoteCANcoder - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); - - // Initial configs for the drive and steer motors and the azimuth encoder; these - // cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API - // documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = - new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a - // relatively - // low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true)); - private static final CANcoderConfiguration encoderInitialConfigs = - new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; - - // CAN bus that the devices are located on; - // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot"); - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.8181818181818183; - - private static final double kDriveGearRatio = 7.363636363636365; - private static final double kSteerGearRatio = 15.42857142857143; - private static final Distance kWheelRadius = Inches.of(2.167); - - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - - private static final int kPigeonId = 1; - - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - - public static final SwerveDrivetrainConstants DrivetrainConstants = - new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - ConstantCreator = - new SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - - // Front Left - private static final int kFrontLeftDriveMotorId = 3; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 1; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); - private static final boolean kFrontLeftSteerMotorInverted = true; - private static final boolean kFrontLeftEncoderInverted = false; - - private static final Distance kFrontLeftXPos = Inches.of(10); - private static final Distance kFrontLeftYPos = Inches.of(10); - - // Front Right - private static final int kFrontRightDriveMotorId = 1; - private static final int kFrontRightSteerMotorId = 0; - private static final int kFrontRightEncoderId = 0; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); - private static final boolean kFrontRightSteerMotorInverted = true; - private static final boolean kFrontRightEncoderInverted = false; - - private static final Distance kFrontRightXPos = Inches.of(10); - private static final Distance kFrontRightYPos = Inches.of(-10); - - // Back Left - private static final int kBackLeftDriveMotorId = 7; - private static final int kBackLeftSteerMotorId = 6; - private static final int kBackLeftEncoderId = 3; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); - private static final boolean kBackLeftSteerMotorInverted = true; - private static final boolean kBackLeftEncoderInverted = false; - - private static final Distance kBackLeftXPos = Inches.of(-10); - private static final Distance kBackLeftYPos = Inches.of(10); - - // Back Right - private static final int kBackRightDriveMotorId = 5; - private static final int kBackRightSteerMotorId = 4; - private static final int kBackRightEncoderId = 2; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); - private static final boolean kBackRightSteerMotorInverted = true; - private static final boolean kBackRightEncoderInverted = false; - - private static final Distance kBackRightXPos = Inches.of(-10); - private static final Distance kBackRightYPos = Inches.of(-10); - - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, - kFrontLeftDriveMotorId, - kFrontLeftEncoderId, - kFrontLeftEncoderOffset, - kFrontLeftXPos, - kFrontLeftYPos, - kInvertLeftSide, - kFrontLeftSteerMotorInverted, - kFrontLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, - kFrontRightDriveMotorId, - kFrontRightEncoderId, - kFrontRightEncoderOffset, - kFrontRightXPos, - kFrontRightYPos, - kInvertRightSide, - kFrontRightSteerMotorInverted, - kFrontRightEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, - kBackLeftDriveMotorId, - kBackLeftEncoderId, - kBackLeftEncoderOffset, - kBackLeftXPos, - kBackLeftYPos, - kInvertLeftSide, - kBackLeftSteerMotorInverted, - kBackLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, - kBackRightDriveMotorId, - kBackRightEncoderId, - kBackRightEncoderOffset, - kBackRightXPos, - kBackRightYPos, - kInvertRightSide, - kBackRightSteerMotorInverted, - kBackRightEncoderInverted); - - /** - * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot - * program,. - */ - // public static CommandSwerveDrivetrain createDrivetrain() { - // return new CommandSwerveDrivetrain( - // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); - // } - - /** - * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. - */ - public static class TunerSwerveDrivetrain - extends SwerveDrivetrain { - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the - * devices themselves. If they need the devices, they can access them through getters in the - * classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules) { - super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules); - } + // PathPlanner config constants + public static final double kRobotMassKG = 74.088; + public static final double kRobotMOI = 6.883; + /** Coefficient of friction */ + public static final double kWheelCOF = 1.2; + + public static final RobotConfig kPathplannerConfig = new RobotConfig( + kRobotMOI, + kRobotMOI, + new ModuleConfig( + ModuleConstants.FrontLeft.WheelRadius, + ModuleConstants.kSpeedAt12Volts.in(MetersPerSecond), + kWheelCOF, + DCMotor.getKrakenX60Foc(1) + .withReduction(ModuleConstants.FrontLeft.DriveMotorGearRatio), + ModuleConstants.FrontLeft.SlipCurrent, + 1), + kModuleTranslations); + + public static final IdleMode kDriveIdleMode = IdleMode.kBrake; + public static final IdleMode kAngleIdleMode = IdleMode.kBrake; + public static final double kDrivePower = 1; + public static final double kAnglePower = .9; + + public static final boolean kInvertGyro = false; // Always ensure Gyro is CCW+ CW- + + // drivetrain constants + public static final double kTrackWidth = Units.inchesToMeters(24.75); + public static final double kWheelBase = Units.inchesToMeters(24.75); + public static final double kWheelDiameter = Units.inchesToMeters(4.0); + public static final double kWheelRadius = kWheelDiameter / 2.0; + public static final double kWheelCircumference = kWheelDiameter * Math.PI; + + // Swerve kinematics, don't change + public static final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics( + new Translation2d(kWheelBase / 2.0, kTrackWidth / 2.0), // front left + new Translation2d(kWheelBase / 2.0, -kTrackWidth / 2.0), // front right + new Translation2d(-kWheelBase / 2.0, kTrackWidth / 2.0), // back left + new Translation2d(-kWheelBase / 2.0, -kTrackWidth / 2.0)); // back right + + // gear ratios + public static final double kDriveGearRatio = (6.12 / 1.0); + public static final double kAngleGearRatio = ((150.0 / 7.0) / 1.0); + + // encoder stuff + // meters per rotation + public static final double kDriveRevToMeters = kWheelCircumference / (kDriveGearRatio); + public static final double kDriveRpmToMetersPerSecond = kDriveRevToMeters / 60; /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the - * devices themselves. If they need the devices, they can access them through getters in the - * classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or - * set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module + * The number of degrees that a single rotation of the turn motor turns the // + * wheel. */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules) { - super( - TalonFX::new, - TalonFX::new, - CANcoder::new, - drivetrainConstants, - odometryUpdateFrequency, - modules); - } + public static final double kDegreesPerTurnRotation = 360 / kAngleGearRatio; - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the - * devices themselves. If they need the devices, they can access them through getters in the - * classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or - * set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation in the - * form [x, y, theta]áµ€, with units in meters and radians - * @param visionStandardDeviation The standard deviation for vision calculation in the form - * [x, y, theta]áµ€, with units in meters and radians - * @param modules Constants for each specific module + // motor inverts, check these + public static final boolean kAngleMotorInvert = true; + public static final InvertedValue kDriveMotorInvert = InvertedValue.CounterClockwise_Positive; + + /* Angle Encoder Invert */ + public static final boolean kCanCoderInvert = false; + + /* Swerve Current Limiting */ + public static final int kAngleContinuousCurrentLimit = 20; + public static final int kAnglePeakCurrentLimit = 40; + public static final double kAnglePeakCurrentDuration = 0.1; + public static final boolean kAngleEnableCurrentLimit = true; + + public static final int kDriveSupplyCurrentLimit = 60; + public static final boolean kDriveSupplyCurrentLimitEnable = true; + public static final int kDriveSupplyCurrentThreshold = 60; + public static final double kDriveSupplyTimeThreshold = 0.1; + + public static final boolean kDriveEnableCurrentLimit = true; + + /* + * These values are used by the drive falcon to ramp in open loop and closed + * loop driving. + * We found a small open loop ramp (0.25) helps with tread wear, tipping, etc */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules) { - super( - TalonFX::new, - TalonFX::new, - CANcoder::new, - drivetrainConstants, - odometryUpdateFrequency, - odometryStandardDeviation, - visionStandardDeviation, - modules); + public static final double kOpenLoopRamp = 0.25; + public static final double kClosedLoopRamp = 0.0; + + /* Angle Motor PID Values */ + public static final double kAngleKP = 0.015; + public static final double kAngleKI = 0; + public static final double kAngleKD = 0; + public static final double kAngleKF = 0; + + /* Drive Motor PID Values */ + + public static final double kDriveKP = 0.01; + public static final double kDriveKI = 0.0; + public static final double kDriveKD = 0.0; + + public static final double kDriveKS = (0.32 / 12); + public static final double kDriveKV = (1.988 / 12); + public static final double kDriveKA = (1.0449 / 12); + + /* Swerve Profiling Values */ + /** Meters per second. */ + public static final double kPhysicalMaxSpeed = 5.0; + + public static final double kMaxTeleDriveSpeed = 4.5; + /** Radians per second. */ + public static final double kPhysicalMaxAngularSpeed = 2 * 2 * Math.PI; + /** Radians per second. */ + public static final double kMaxTeleAngularSpeed = kPhysicalMaxAngularSpeed / 2; + + public static final double kMaxAngularAccelerationSpeed = 4 / Math.PI; + /** Radians per second. */ + public static final double kMaxTeleAngularAccelerationSpeed = kMaxAngularAccelerationSpeed / 2; + + public static final double kDeadband = 0.08; + + public static final Map kDistances = Map.of( + 0, 0.0, + 1, 1.0, + 2, 2.0, + 3, 3.0, + 4, 4.0); + + public static class ModuleConstants { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with + // the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100) + .withKI(0) + .withKD(0.5) + .withKS(0.1) + .withKV(1.91) + .withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0) + .withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to + // RemoteCANcoder + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current kSlipCurrent = Amps.of(120.0); + + // Initial configs for the drive and steer motors and the azimuth encoder; these + // cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API + // documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a + // relatively + // low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true)); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 3.8181818181818183; + + private static final double kDriveGearRatio = 7.363636363636365; + private static final double kSteerGearRatio = 15.42857142857143; + private static final Distance kWheelRadius = Inches.of(2.167); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 1; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + // Front Left + private static final int kFrontLeftDriveMotorId = 3; + private static final int kFrontLeftSteerMotorId = 2; + private static final int kFrontLeftEncoderId = 1; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(10); + private static final Distance kFrontLeftYPos = Inches.of(10); + + // Front Right + private static final int kFrontRightDriveMotorId = 1; + private static final int kFrontRightSteerMotorId = 0; + private static final int kFrontRightEncoderId = 0; + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(10); + private static final Distance kFrontRightYPos = Inches.of(-10); + + // Back Left + private static final int kBackLeftDriveMotorId = 7; + private static final int kBackLeftSteerMotorId = 6; + private static final int kBackLeftEncoderId = 3; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); + private static final boolean kBackLeftSteerMotorInverted = true; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-10); + private static final Distance kBackLeftYPos = Inches.of(10); + + // Back Right + private static final int kBackRightDriveMotorId = 5; + private static final int kBackRightSteerMotorId = 4; + private static final int kBackRightEncoderId = 2; + private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-10); + private static final Distance kBackRightYPos = Inches.of(-10); + + public static final SwerveModuleConstants FrontLeft = ConstantCreator + .createModuleConstants( + kFrontLeftSteerMotorId, + kFrontLeftDriveMotorId, + kFrontLeftEncoderId, + kFrontLeftEncoderOffset, + kFrontLeftXPos, + kFrontLeftYPos, + kInvertLeftSide, + kFrontLeftSteerMotorInverted, + kFrontLeftEncoderInverted); + public static final SwerveModuleConstants FrontRight = ConstantCreator + .createModuleConstants( + kFrontRightSteerMotorId, + kFrontRightDriveMotorId, + kFrontRightEncoderId, + kFrontRightEncoderOffset, + kFrontRightXPos, + kFrontRightYPos, + kInvertRightSide, + kFrontRightSteerMotorInverted, + kFrontRightEncoderInverted); + public static final SwerveModuleConstants BackLeft = ConstantCreator + .createModuleConstants( + kBackLeftSteerMotorId, + kBackLeftDriveMotorId, + kBackLeftEncoderId, + kBackLeftEncoderOffset, + kBackLeftXPos, + kBackLeftYPos, + kInvertLeftSide, + kBackLeftSteerMotorInverted, + kBackLeftEncoderInverted); + public static final SwerveModuleConstants BackRight = ConstantCreator + .createModuleConstants( + kBackRightSteerMotorId, + kBackRightDriveMotorId, + kBackRightEncoderId, + kBackRightEncoderOffset, + kBackRightXPos, + kBackRightYPos, + kInvertRightSide, + kBackRightSteerMotorInverted, + kBackRightEncoderInverted); + + /** + * Creates a CommandSwerveDrivetrain instance. This should only be called once + * in your robot + * program,. + */ + // public static CommandSwerveDrivetrain createDrivetrain() { + // return new CommandSwerveDrivetrain( + // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); + // } + + /** + * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected + * device types. + */ + public static class TunerSwerveDrivetrain + extends SwerveDrivetrain { + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

+ * This constructs the underlying hardware devices, so users should not + * construct the + * devices themselves. If they need the devices, they can access them through + * getters in the + * classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules) { + super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

+ * This constructs the underlying hardware devices, so users should not + * construct the + * devices themselves. If they need the devices, they can access them through + * getters in the + * classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or + * set to 0 Hz, this is 250 Hz on CAN FD, and 100 + * Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules) { + super( + TalonFX::new, + TalonFX::new, + CANcoder::new, + drivetrainConstants, + odometryUpdateFrequency, + modules); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

+ * This constructs the underlying hardware devices, so users should not + * construct the + * devices themselves. If they need the devices, they can access them through + * getters in the + * classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve + * drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or + * set to 0 Hz, this is 250 Hz on CAN FD, and + * 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry + * calculation in the + * form [x, y, theta]áµ€, with units in meters + * and radians + * @param visionStandardDeviation The standard deviation for vision + * calculation in the form + * [x, y, theta]áµ€, with units in meters and + * radians + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { + super( + TalonFX::new, + TalonFX::new, + CANcoder::new, + drivetrainConstants, + odometryUpdateFrequency, + odometryStandardDeviation, + visionStandardDeviation, + modules); + } + } } - } } - } - - public class ModuleConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with - // the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = - new Slot0Configs() - .withKP(100) - .withKI(0) - .withKD(0.5) - .withKS(0.1) - .withKV(1.91) - .withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = - new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = - DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = - SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to - // RemoteCANcoder - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); - - // Initial configs for the drive and steer motors and the azimuth encoder; these - // cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API - // documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = - new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a - // relatively - // low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true)); - private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; - - // CAN bus that the devices are located on; - // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot"); - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.8181818181818183; - - private static final double kDriveGearRatio = 7.363636363636365; - private static final double kSteerGearRatio = 15.42857142857143; - private static final Distance kWheelRadius = Inches.of(2.167); - - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - - private static final int kPigeonId = 1; - - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - - public static final SwerveDrivetrainConstants DrivetrainConstants = - new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - ConstantCreator = - new SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() + + public class ModuleConstants { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with + // the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100) + .withKI(0) + .withKD(0.5) + .withKS(0.1) + .withKV(1.91) + .withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0) + .withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to + // RemoteCANcoder + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current kSlipCurrent = Amps.of(120.0); + + // Initial configs for the drive and steer motors and the azimuth encoder; these + // cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API + // documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a + // relatively + // low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true)); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 3.8181818181818183; + + private static final double kDriveGearRatio = 7.363636363636365; + private static final double kSteerGearRatio = 15.42857142857143; + private static final Distance kWheelRadius = Inches.of(2.167); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 1; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() .withDriveMotorGearRatio(kDriveGearRatio) .withSteerMotorGearRatio(kSteerGearRatio) .withCouplingGearRatio(kCoupleRatio) @@ -693,248 +682,255 @@ public class ModuleConstants { .withSteerFrictionVoltage(kSteerFrictionVoltage) .withDriveFrictionVoltage(kDriveFrictionVoltage); - // Front Left - private static final int kFrontLeftDriveMotorId = 3; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 1; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); - private static final boolean kFrontLeftSteerMotorInverted = true; - private static final boolean kFrontLeftEncoderInverted = false; - - private static final Distance kFrontLeftXPos = Inches.of(10); - private static final Distance kFrontLeftYPos = Inches.of(10); - - // Front Right - private static final int kFrontRightDriveMotorId = 1; - private static final int kFrontRightSteerMotorId = 0; - private static final int kFrontRightEncoderId = 0; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); - private static final boolean kFrontRightSteerMotorInverted = true; - private static final boolean kFrontRightEncoderInverted = false; - - private static final Distance kFrontRightXPos = Inches.of(10); - private static final Distance kFrontRightYPos = Inches.of(-10); - - // Back Left - private static final int kBackLeftDriveMotorId = 7; - private static final int kBackLeftSteerMotorId = 6; - private static final int kBackLeftEncoderId = 3; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); - private static final boolean kBackLeftSteerMotorInverted = true; - private static final boolean kBackLeftEncoderInverted = false; - - private static final Distance kBackLeftXPos = Inches.of(-10); - private static final Distance kBackLeftYPos = Inches.of(10); - - // Back Right - private static final int kBackRightDriveMotorId = 5; - private static final int kBackRightSteerMotorId = 4; - private static final int kBackRightEncoderId = 2; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); - private static final boolean kBackRightSteerMotorInverted = true; - private static final boolean kBackRightEncoderInverted = false; - - private static final Distance kBackRightXPos = Inches.of(-10); - private static final Distance kBackRightYPos = Inches.of(-10); - - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, - kFrontLeftDriveMotorId, - kFrontLeftEncoderId, - kFrontLeftEncoderOffset, - kFrontLeftXPos, - kFrontLeftYPos, - kInvertLeftSide, - kFrontLeftSteerMotorInverted, - kFrontLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, - kFrontRightDriveMotorId, - kFrontRightEncoderId, - kFrontRightEncoderOffset, - kFrontRightXPos, - kFrontRightYPos, - kInvertRightSide, - kFrontRightSteerMotorInverted, - kFrontRightEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, - kBackLeftDriveMotorId, - kBackLeftEncoderId, - kBackLeftEncoderOffset, - kBackLeftXPos, - kBackLeftYPos, - kInvertLeftSide, - kBackLeftSteerMotorInverted, - kBackLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, - kBackRightDriveMotorId, - kBackRightEncoderId, - kBackRightEncoderOffset, - kBackRightXPos, - kBackRightYPos, - kInvertRightSide, - kBackRightSteerMotorInverted, - kBackRightEncoderInverted); - - /** - * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot - * program,. - */ - // public static CommandSwerveDrivetrain createDrivetrain() { - // return new CommandSwerveDrivetrain( - // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); - // } - - /** - * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. - */ - public static class TunerSwerveDrivetrain extends SwerveDrivetrain { - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the - * devices themselves. If they need the devices, they can access them through getters in the - * classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules) { - super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the - * devices themselves. If they need the devices, they can access them through getters in the - * classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or - * set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules) { - super( - TalonFX::new, - TalonFX::new, - CANcoder::new, - drivetrainConstants, - odometryUpdateFrequency, - modules); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the - * devices themselves. If they need the devices, they can access them through getters in the - * classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or - * set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation in the - * form [x, y, theta]áµ€, with units in meters and radians - * @param visionStandardDeviation The standard deviation for vision calculation in the form - * [x, y, theta]áµ€, with units in meters and radians - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules) { - super( - TalonFX::new, - TalonFX::new, - CANcoder::new, - drivetrainConstants, - odometryUpdateFrequency, - odometryStandardDeviation, - visionStandardDeviation, - modules); - } + // Front Left + private static final int kFrontLeftDriveMotorId = 3; + private static final int kFrontLeftSteerMotorId = 2; + private static final int kFrontLeftEncoderId = 1; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(10); + private static final Distance kFrontLeftYPos = Inches.of(10); + + // Front Right + private static final int kFrontRightDriveMotorId = 1; + private static final int kFrontRightSteerMotorId = 0; + private static final int kFrontRightEncoderId = 0; + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(10); + private static final Distance kFrontRightYPos = Inches.of(-10); + + // Back Left + private static final int kBackLeftDriveMotorId = 7; + private static final int kBackLeftSteerMotorId = 6; + private static final int kBackLeftEncoderId = 3; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); + private static final boolean kBackLeftSteerMotorInverted = true; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-10); + private static final Distance kBackLeftYPos = Inches.of(10); + + // Back Right + private static final int kBackRightDriveMotorId = 5; + private static final int kBackRightSteerMotorId = 4; + private static final int kBackRightEncoderId = 2; + private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-10); + private static final Distance kBackRightYPos = Inches.of(-10); + + public static final SwerveModuleConstants FrontLeft = ConstantCreator + .createModuleConstants( + kFrontLeftSteerMotorId, + kFrontLeftDriveMotorId, + kFrontLeftEncoderId, + kFrontLeftEncoderOffset, + kFrontLeftXPos, + kFrontLeftYPos, + kInvertLeftSide, + kFrontLeftSteerMotorInverted, + kFrontLeftEncoderInverted); + public static final SwerveModuleConstants FrontRight = ConstantCreator + .createModuleConstants( + kFrontRightSteerMotorId, + kFrontRightDriveMotorId, + kFrontRightEncoderId, + kFrontRightEncoderOffset, + kFrontRightXPos, + kFrontRightYPos, + kInvertRightSide, + kFrontRightSteerMotorInverted, + kFrontRightEncoderInverted); + public static final SwerveModuleConstants BackLeft = ConstantCreator + .createModuleConstants( + kBackLeftSteerMotorId, + kBackLeftDriveMotorId, + kBackLeftEncoderId, + kBackLeftEncoderOffset, + kBackLeftXPos, + kBackLeftYPos, + kInvertLeftSide, + kBackLeftSteerMotorInverted, + kBackLeftEncoderInverted); + public static final SwerveModuleConstants BackRight = ConstantCreator + .createModuleConstants( + kBackRightSteerMotorId, + kBackRightDriveMotorId, + kBackRightEncoderId, + kBackRightEncoderOffset, + kBackRightXPos, + kBackRightYPos, + kInvertRightSide, + kBackRightSteerMotorInverted, + kBackRightEncoderInverted); + + /** + * Creates a CommandSwerveDrivetrain instance. This should only be called once + * in your robot + * program,. + */ + // public static CommandSwerveDrivetrain createDrivetrain() { + // return new CommandSwerveDrivetrain( + // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); + // } + + /** + * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected + * device types. + */ + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

+ * This constructs the underlying hardware devices, so users should not + * construct the + * devices themselves. If they need the devices, they can access them through + * getters in the + * classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules) { + super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

+ * This constructs the underlying hardware devices, so users should not + * construct the + * devices themselves. If they need the devices, they can access them through + * getters in the + * classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or + * set to 0 Hz, this is 250 Hz on CAN FD, and 100 + * Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules) { + super( + TalonFX::new, + TalonFX::new, + CANcoder::new, + drivetrainConstants, + odometryUpdateFrequency, + modules); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

+ * This constructs the underlying hardware devices, so users should not + * construct the + * devices themselves. If they need the devices, they can access them through + * getters in the + * classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve + * drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or + * set to 0 Hz, this is 250 Hz on CAN FD, and + * 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry + * calculation in the + * form [x, y, theta]áµ€, with units in meters + * and radians + * @param visionStandardDeviation The standard deviation for vision + * calculation in the form + * [x, y, theta]áµ€, with units in meters and + * radians + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { + super( + TalonFX::new, + TalonFX::new, + CANcoder::new, + drivetrainConstants, + odometryUpdateFrequency, + odometryStandardDeviation, + visionStandardDeviation, + modules); + } + } } - } - - public class VisionConstants { - // AprilTag layout - public static AprilTagFieldLayout aprilTagLayout = - AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); - - // Camera names, must match names configured on coprocessor - public static String camera0Name = "camera_0"; - public static String camera1Name = "camera_1"; - - // Robot to camera transforms - // (Not used by Limelight, configure in web UI instead) - public static Transform3d robotToCamera0 = - new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); - public static Transform3d robotToCamera1 = - new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); - - // Basic filtering thresholds - public static double maxAmbiguity = 0.3; - public static double maxZError = 0.75; - - // Standard deviation baselines, for 1 meter distance and 1 tag - // (Adjusted automatically based on distance and # of tags) - public static double linearStdDevBaseline = 0.02; // Meters - public static double angularStdDevBaseline = 0.06; // Radians - - // Standard deviation multipliers for each camera - // (Adjust to trust some cameras more than others) - public static double[] cameraStdDevFactors = - new double[] { - 1.0, // Camera 0 - 1.0 // Camera 1 + + public class VisionConstants { + // AprilTag layout + public static AprilTagFieldLayout aprilTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + + // Camera names, must match names configured on coprocessor + public static String camera0Name = "camera_0"; + public static String camera1Name = "camera_1"; + + // Robot to camera transforms + // (Not used by Limelight, configure in web UI instead) + public static Transform3d robotToCamera0 = new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); + public static Transform3d robotToCamera1 = new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); + + // Basic filtering thresholds + public static double maxAmbiguity = 0.3; + public static double maxZError = 0.75; + + // Standard deviation baselines, for 1 meter distance and 1 tag + // (Adjusted automatically based on distance and # of tags) + public static double linearStdDevBaseline = 0.02; // Meters + public static double angularStdDevBaseline = 0.06; // Radians + + // Standard deviation multipliers for each camera + // (Adjust to trust some cameras more than others) + public static double[] cameraStdDevFactors = new double[] { + 1.0, // Camera 0 + 1.0 // Camera 1 }; - // Multipliers to apply for MegaTag 2 observations - public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve - public static double angularStdDevMegatag2Factor = - Double.POSITIVE_INFINITY; // No rotation data available - } - - public static class IntakeConstants { - public static final int kPivotMotorID = 8; - public static final int kRollerMotorID = 9; - - public static final double kPivotMotorSpeed = 0.5; - public static final double kRollerMotorSpeed = 0.5; - - // Change Gear Ratios later - public static final double kPivotMotorGearRatio = 1.0; - public static final double kRollerMotorGearRatio = 1.0; - } - - public static class OperatorConstants { - public static final Joystick auxStick = new Joystick(7); - public static JoystickButton kIntakeButton1 = new JoystickButton(auxStick, 4); - public static JoystickButton kIntakeButton2 = new JoystickButton(auxStick, 5); - public static JoystickButton kIntakeButton3 = new JoystickButton(auxStick, 6); - public static JoystickButton kIntakeButton4 = new JoystickButton(auxStick, 7); - } + // Multipliers to apply for MegaTag 2 observations + public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve + public static double angularStdDevMegatag2Factor = Double.POSITIVE_INFINITY; // No rotation data available + } + + public static class IntakeConstants { + public static final int kPivotMotorID = 8; + public static final int kRollerMotorID = 9; + + public static final double kPivotMotorSpeed = 0.5; + public static final double kRollerMotorSpeed = 0.5; + + // Change Gear Ratios later + public static final double kPivotMotorGearRatio = 1.0; + public static final double kRollerMotorGearRatio = 1.0; + } + + public static class OperatorConstants { + public static final Joystick auxStick = new Joystick(7); + public static JoystickButton kIntakeButton1 = new JoystickButton(auxStick, 4); + public static JoystickButton kIntakeButton2 = new JoystickButton(auxStick, 5); + public static JoystickButton kIntakeButton3 = new JoystickButton(auxStick, 6); + public static JoystickButton kIntakeButton4 = new JoystickButton(auxStick, 7); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8e699a0..df9520b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -37,34 +37,37 @@ public class RobotContainer { public RobotContainer() { switch (Constants.kCurrentMode) { case REAL: - drive = - new Drive( - new GyroIOPigeon2(), - new ModuleIOTalonFX(ModuleConstants.FrontLeft), - new ModuleIOTalonFX(ModuleConstants.FrontRight), - new ModuleIOTalonFX(ModuleConstants.BackLeft), - new ModuleIOTalonFX(ModuleConstants.BackRight)); + drive = new Drive( + new GyroIOPigeon2(), + new ModuleIOTalonFX(ModuleConstants.FrontLeft), + new ModuleIOTalonFX(ModuleConstants.FrontRight), + new ModuleIOTalonFX(ModuleConstants.BackLeft), + new ModuleIOTalonFX(ModuleConstants.BackRight)); vision = new Vision(null, null); break; case SIM: - drive = - new Drive( - new GyroIO() {}, - new ModuleIOSim(ModuleConstants.FrontLeft), - new ModuleIOSim(ModuleConstants.FrontRight), - new ModuleIOSim(ModuleConstants.BackLeft), - new ModuleIOSim(ModuleConstants.BackRight)); + drive = new Drive( + new GyroIO() { + }, + new ModuleIOSim(ModuleConstants.FrontLeft), + new ModuleIOSim(ModuleConstants.FrontRight), + new ModuleIOSim(ModuleConstants.BackLeft), + new ModuleIOSim(ModuleConstants.BackRight)); vision = new Vision(null, null); break; case REPLAY: default: - drive = - new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}); + drive = new Drive( + new GyroIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }); vision = new Vision(null, new CameraIO[] {}); break; } @@ -86,8 +89,7 @@ private void configureBindings() { () -> -driver.getLeftX(), // ySupplier () -> { Pose2d robotPose = RobotState.getInstance().getEstimatedPose(); - Translation2d target = - AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d()); + Translation2d target = AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d()); Translation2d delta = target.minus(robotPose.getTranslation()); @@ -110,9 +112,8 @@ private void configureBindings() { } public void robotPeriodic() { - OdometryObservation obs = - new OdometryObservation( - Timer.getTimestamp(), drive.getModulePositions(), drive.getRawGyroRotation()); + OdometryObservation obs = new OdometryObservation( + Timer.getTimestamp(), drive.getModulePositions(), drive.getRawGyroRotation()); RobotState.getInstance().addOdometryObservation(obs); } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index ad9b543..d0e1322 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -19,10 +19,12 @@ public class Intake extends SubsystemBase { public Intake(IntakeIO io) { this.io = io; } + /** * Command to run the pivot * - * @return runs the pivot at a speed on every iteration until end when it stops the running + * @return runs the pivot at a speed on every iteration until end when it stops + * the running */ public Command runPivot() { return Commands.runEnd( @@ -30,10 +32,12 @@ public Command runPivot() { () -> io.setPivotSpeed(0.0), this); } + /** * Command to run the pivot back * - * @return runs the pivot at a speed on every iteration until end when it stops the running + * @return runs the pivot at a speed on every iteration until end when it stops + * the running */ public Command runPivotBack() { return Commands.runEnd( @@ -41,10 +45,12 @@ public Command runPivotBack() { () -> io.setPivotSpeed(0.0), this); } + /** * Command to run the feeder * - * @return runs the feeder at a speed on every iteration until end when it stops the running + * @return runs the feeder at a speed on every iteration until end when it stops + * the running */ public Command runFeeder() { return Commands.runEnd( @@ -56,7 +62,8 @@ public Command runFeeder() { /** * Command to run the feeder backward * - * @return runs the feeder at a speed on every iteration until end when it stops the running + * @return runs the feeder at a speed on every iteration until end when it stops + * the running */ public Command runFeederBack() { return Commands.runEnd( @@ -67,21 +74,21 @@ public Command runFeederBack() { // potential sequences for commands in future // public Command extendArmSequence() { - // return Commands.run(() -> runArm(.5), this) - // .andThen(Commands.waitUntil()) - // .finallyDo(Commands.runOnce(() -> runArm(0))); + // return Commands.run(() -> runArm(.5), this) + // .andThen(Commands.waitUntil()) + // .finallyDo(Commands.runOnce(() -> runArm(0))); // } // public Command retractArmSequence() { - // return Commands.run(() -> runArm(-0.5), this) - // .andThen(Commands.waitUntil()) - // .finallyDo(Commands.runOnce(() -> runArm(0))); + // return Commands.run(() -> runArm(-0.5), this) + // .andThen(Commands.waitUntil()) + // .finallyDo(Commands.runOnce(() -> runArm(0))); // } // public Command runFeederSequence() { - // return Commands.run(() -> runFeeder(.5), this) - // .andThen(Commands.waitUntil()) - // .finallyDo(Commands.runOnce(() -> runArm(0))); + // return Commands.run(() -> runFeeder(.5), this) + // .andThen(Commands.waitUntil()) + // .finallyDo(Commands.runOnce(() -> runArm(0))); // } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index fd0a2cc..c111976 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -3,7 +3,8 @@ import org.littletonrobotics.junction.AutoLog; /** - * The {@code IntakeIO} class provides methods for interacting with the intake motors and updating + * The {@code IntakeIO} class provides methods for interacting with the intake + * motors and updating * the intake inputs. * * @author Ryan Hefferon @@ -12,7 +13,8 @@ * @author Julien Precourt */ public interface IntakeIO { - default void updateInputs(IntakeIOInputs inputs) {} + default void updateInputs(IntakeIOInputs inputs) { + } @AutoLog public static class IntakeIOInputs { @@ -25,16 +27,20 @@ public static class IntakeIOInputs { public double pivotCurrentDrawAmps = 0.0; public double wheelCurrentDrawAmps = 0.0; } + /** * method to set the speed of the pivot * * @param speed determines the speed of the pivot on a scale of -1 to 1 */ - default void setPivotSpeed(double speed) {} + default void setPivotSpeed(double speed) { + } + /** * method to set the speed of the wheel * * @param speed determines the speed of the wheel on a scale of -1 to 1 */ - default void setWheelSpeed(double speed) {} + default void setWheelSpeed(double speed) { + } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java index ff48655..79f728f 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java @@ -2,6 +2,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; +import com.revrobotics.PersistMode; import com.revrobotics.RelativeEncoder; import com.revrobotics.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; @@ -20,7 +21,7 @@ public class IntakeIOHardware implements IntakeIO { public IntakeIOHardware() { pivotConfig = new SparkMaxConfig(); wheelMotor.getConfigurator().apply(wheelMotorConfig); - pivotMotor.configure(pivotConfig, ResetMode.kNoResetSafeParameters, null); + pivotMotor.configure(pivotConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); } @Override @@ -35,10 +36,8 @@ public void setWheelSpeed(double speed) { @Override public void updateInputs(IntakeIOInputs inputs) { - inputs.pivotVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(pivotEncoder.getVelocity()); - inputs.wheelVelocityRadPerSec = - Units.rotationsToRadians(wheelMotor.getVelocity().getValueAsDouble()); + inputs.pivotVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(pivotEncoder.getVelocity()); + inputs.wheelVelocityRadPerSec = Units.rotationsToRadians(wheelMotor.getVelocity().getValueAsDouble()); inputs.pivotPositionRad = Units.rotationsToRadians(pivotEncoder.getPosition()); inputs.wheelPositionRad = Units.rotationsToRadians(wheelMotor.getPosition().getValueAsDouble()); inputs.pivotAppliedVolts = pivotMotor.getAppliedOutput(); From a65cb4aaf7a83eec42ea3e121817f5a9ec19d4ab Mon Sep 17 00:00:00 2001 From: Matthew McGrath Date: Thu, 12 Feb 2026 19:13:15 -0500 Subject: [PATCH 33/61] changed the units of position and velocity inputs to radians and radians per second --- .../robot/subsystems/intake/IntakeIOSim.java | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 8124a74..d4f8313 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.Constants.IntakeConstants; @@ -13,23 +14,22 @@ public class IntakeIOSim implements IntakeIO { private final DCMotorSim pivotSim; private final DCMotorSim wheelSim; - // private final PIDController pid = new PIDController(1, 0, 0, Constants.kLoopPeriodSeconds); + // private final PIDController pid = new PIDController(1, 0, 0, + // Constants.kLoopPeriodSeconds); private double pivotAppliedVolts = 0.0; private double wheelAppliedVolts = 0.0; public IntakeIOSim() { - pivotSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - pivotGearbox, 0.025, IntakeConstants.kPivotMotorGearRatio), - pivotGearbox); - - wheelSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - wheelGearbox, 0.025, IntakeConstants.kRollerMotorGearRatio), - wheelGearbox); + pivotSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem( + pivotGearbox, 0.025, IntakeConstants.kPivotMotorGearRatio), + pivotGearbox); + + wheelSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem( + wheelGearbox, 0.025, IntakeConstants.kRollerMotorGearRatio), + wheelGearbox); } @Override @@ -44,11 +44,11 @@ public void updateInputs(IntakeIOInputs inputs) { wheelSim.setInputVoltage(wheelAppliedVolts); wheelSim.update(0.02); - inputs.pivotPositionRad = pivotSim.getAngularPositionRotations(); - inputs.pivotVelocityRadPerSec = pivotSim.getAngularVelocityRPM(); + inputs.pivotPositionRad = Units.rotationsToRadians(pivotSim.getAngularPositionRotations()); + inputs.pivotVelocityRadPerSec = Units.rotationsToRadians(pivotSim.getAngularVelocityRPM()); - inputs.wheelPositionRad = wheelSim.getAngularPositionRotations(); - inputs.wheelVelocityRadPerSec = wheelSim.getAngularVelocityRPM(); + inputs.wheelPositionRad = Units.rotationsToRadians(wheelSim.getAngularPositionRotations()); + inputs.wheelVelocityRadPerSec = Units.rotationsToRadians(wheelSim.getAngularVelocityRPM()); } @Override From ad054cf8b74e5b01974f15b45c2f68a024cd70d9 Mon Sep 17 00:00:00 2001 From: Maxwell Morgan Date: Mon, 16 Feb 2026 09:34:18 -0500 Subject: [PATCH 34/61] Split constants file --- src/main/java/frc/robot/Constants.java | 428 ------------------ src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/RobotState.java | 3 +- src/main/java/frc/robot/RobotVisualizer.java | 4 +- .../frc/robot/commands/DriveCommands.java | 2 +- .../frc/robot/subsystems/drive/Drive.java | 3 +- .../subsystems/drive/DriveConstants.java | 315 +++++++++++++ .../robot/subsystems/drive/GyroIONavX.java | 1 - .../robot/subsystems/drive/GyroIOPigeon2.java | 4 +- .../subsystems/drive/ModuleIOTalonFX.java | 4 +- .../subsystems/drive/ModuleIOTalonFXS.java | 4 +- .../drive/PhoenixOdometryThread.java | 4 +- .../subsystems/shooter/ShooterConstants.java | 80 ++++ .../shooter/TrajectoryCalculator.java | 3 +- .../subsystems/shooter/flywheel/Flywheel.java | 3 +- .../shooter/flywheel/FlywheelIOSim.java | 2 +- .../shooter/flywheel/FlywheelIOTalonFX.java | 2 +- .../subsystems/shooter/hood/HoodIOSim.java | 2 +- .../shooter/hood/HoodIOSparkMax.java | 8 +- .../subsystems/shooter/turret/Turret.java | 3 +- .../shooter/turret/TurretIOSim.java | 2 +- .../shooter/turret/TurretIOSparkMax.java | 8 +- .../vision/CameraIOPhotonVision.java | 4 +- .../vision/CameraIOPhotonVisionSim.java | 6 +- .../frc/robot/subsystems/vision/Vision.java | 26 +- .../subsystems/vision/VisionConstants.java | 45 ++ 26 files changed, 491 insertions(+), 477 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/drive/DriveConstants.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionConstants.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fc3a911..db8e1b6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,50 +7,7 @@ package frc.robot; -import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.KilogramSquareMeters; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Rotations; -import static edu.wpi.first.units.Units.Volts; - -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.Pigeon2Configuration; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; -import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; -import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; -import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; -import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; -import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; -import com.pathplanner.lib.config.ModuleConfig; -import com.pathplanner.lib.config.RobotConfig; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.MomentOfInertia; -import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotBase; -import frc.robot.subsystems.drive.Drive; -import frc.robot.util.GeomUtil; /** * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running @@ -83,389 +40,4 @@ public static void disableHAL() { kDisableHAL = true; } - public static final class DriveConstants { - public static final SwerveDriveKinematics kSwerveKinematics = - new SwerveDriveKinematics(Drive.getModuleTranslations()); - - public static final double kOdometryFrequency = - ModuleConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0; - public static final double kDriveBaseRadius = - Math.max( - Math.max( - Math.hypot( - ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), - Math.hypot( - ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY)), - Math.max( - Math.hypot(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), - Math.hypot( - ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY))); - - public static final Translation2d[] kModuleTranslations = - new Translation2d[] { - new Translation2d( - ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), - new Translation2d( - ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY), - new Translation2d(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), - new Translation2d( - ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY) - }; - - // TODO: Update for robot - // PathPlanner config constants - public static final double kRobotMassKG = 74.088; - public static final double kRobotMOI = 6.883; - /** Coefficient of friction */ - public static final double kWheelCOF = 1.2; - - public static final RobotConfig kPathplannerConfig = - new RobotConfig( - kRobotMOI, - kRobotMOI, - new ModuleConfig( - ModuleConstants.FrontLeft.WheelRadius, - ModuleConstants.kSpeedAt12Volts.in(MetersPerSecond), - kWheelCOF, - DCMotor.getKrakenX60(1) - .withReduction(ModuleConstants.FrontLeft.DriveMotorGearRatio), - ModuleConstants.FrontLeft.SlipCurrent, - 1), - kModuleTranslations); - - public static final class ModuleConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with - // the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - // TODO: Update for robot - private static final Slot0Configs steerGains = - new Slot0Configs() - .withKP(100) - .withKI(0) - .withKD(0.5) - .withKS(0.1) - .withKV(1.91) - .withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - // TODO: Update for robot - private static final Slot0Configs driveGains = - new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = - ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = - ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = - DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = - SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to - // RemoteCANcoder - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - // TODO: Update for robot - private static final Current kSlipCurrent = Amps.of(120.0); - - // Initial configs for the drive and steer motors and the azimuth encoder; these - // cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API - // documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = - new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a - // relatively - // low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true)); - private static final CANcoderConfiguration encoderInitialConfigs = - new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; - - // CAN bus that the devices are located on; - // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot"); - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - // TODO: Update for robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - // TODO: Update for robot - private static final double kCoupleRatio = 3.8181818181818183; - // TODO: Update for robot - private static final double kDriveGearRatio = 7.363636363636365; - private static final double kSteerGearRatio = 15.42857142857143; - private static final Distance kWheelRadius = Inches.of(2.167); - // TODO: Update for robot - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - // TODO: Update for robot - private static final int kPigeonId = 1; - - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - - public static final SwerveDrivetrainConstants DrivetrainConstants = - new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - ConstantCreator = - new SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - - // TODO: Update for robot - // Front Left - private static final int kFrontLeftDriveMotorId = 3; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 1; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); - private static final boolean kFrontLeftSteerMotorInverted = true; - private static final boolean kFrontLeftEncoderInverted = false; - - private static final Distance kFrontLeftXPos = Inches.of(10); - private static final Distance kFrontLeftYPos = Inches.of(10); - // TODO: Update for robot - // Front Right - private static final int kFrontRightDriveMotorId = 1; - private static final int kFrontRightSteerMotorId = 0; - private static final int kFrontRightEncoderId = 0; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); - private static final boolean kFrontRightSteerMotorInverted = true; - private static final boolean kFrontRightEncoderInverted = false; - - private static final Distance kFrontRightXPos = Inches.of(10); - private static final Distance kFrontRightYPos = Inches.of(-10); - // TODO: Update for robot - // Back Left - private static final int kBackLeftDriveMotorId = 7; - private static final int kBackLeftSteerMotorId = 6; - private static final int kBackLeftEncoderId = 3; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); - private static final boolean kBackLeftSteerMotorInverted = true; - private static final boolean kBackLeftEncoderInverted = false; - - private static final Distance kBackLeftXPos = Inches.of(-10); - private static final Distance kBackLeftYPos = Inches.of(10); - // TODO: Update for robot - // Back Right - private static final int kBackRightDriveMotorId = 5; - private static final int kBackRightSteerMotorId = 4; - private static final int kBackRightEncoderId = 2; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); - private static final boolean kBackRightSteerMotorInverted = true; - private static final boolean kBackRightEncoderInverted = false; - - private static final Distance kBackRightXPos = Inches.of(-10); - private static final Distance kBackRightYPos = Inches.of(-10); - - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, - kFrontLeftDriveMotorId, - kFrontLeftEncoderId, - kFrontLeftEncoderOffset, - kFrontLeftXPos, - kFrontLeftYPos, - kInvertLeftSide, - kFrontLeftSteerMotorInverted, - kFrontLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, - kFrontRightDriveMotorId, - kFrontRightEncoderId, - kFrontRightEncoderOffset, - kFrontRightXPos, - kFrontRightYPos, - kInvertRightSide, - kFrontRightSteerMotorInverted, - kFrontRightEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, - kBackLeftDriveMotorId, - kBackLeftEncoderId, - kBackLeftEncoderOffset, - kBackLeftXPos, - kBackLeftYPos, - kInvertLeftSide, - kBackLeftSteerMotorInverted, - kBackLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, - kBackRightDriveMotorId, - kBackRightEncoderId, - kBackRightEncoderOffset, - kBackRightXPos, - kBackRightYPos, - kInvertRightSide, - kBackRightSteerMotorInverted, - kBackRightEncoderInverted); - } - } - - public static final class VisionConstants { - // AprilTag layout - public static AprilTagFieldLayout aprilTagLayout = - AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); - - // Camera names, must match names configured on coprocessor - public static String camera0Name = "camera_0"; - public static String camera1Name = "camera_1"; - - // Robot to camera transforms - // (Not used by Limelight, configure in web UI instead) - public static Transform3d robotToCamera0 = - new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); - public static Transform3d robotToCamera1 = - new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); - - // Basic filtering thresholds - public static double maxAmbiguity = 0.3; - public static double maxZError = 0.75; - - // Standard deviation baselines, for 1 meter distance and 1 tag - // (Adjusted automatically based on distance and # of tags) - public static double linearStdDevBaseline = 0.02; // Meters - public static double angularStdDevBaseline = 0.06; // Radians - - // Standard deviation multipliers for each camera - // (Adjust to trust some cameras more than others) - public static double[] cameraStdDevFactors = - new double[] { - 1.0, // Camera 0 - 1.0 // Camera 1 - }; - - // Multipliers to apply for MegaTag 2 observations - public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve - public static double angularStdDevMegatag2Factor = - Double.POSITIVE_INFINITY; // No rotation data available - } - - public static final class ShooterConstants { - - public static final class TurretConstants { - public static final double kGearRatio = 10 / 1; - public static final double kMinTurretAngleRad = Units.degreesToRadians(-180); - public static final double kMaxTurretAngleRad = Units.degreesToRadians(180); - - public static final double kLeftMotorId = 12; - public static final double kRightMotorId = 13; - - // +X = Forward, +Y = Left - public static final Transform3d kRobotToLeftTurret = - new Transform3d(Inches.of(3.749), Inches.of(8.186), Inches.of(13.401), Rotation3d.kZero); - - public static final Transform3d kRobotToRightTurret = - new Transform3d(Inches.of(3.749), Inches.of(-8.314), Inches.of(13.401), Rotation3d.kZero); - } - - public static final class HoodConstants { - public static final double kTurretToHoodInches = 1.878; - public static final double kGearRatio = 100 / 1; - - public static final Transform3d kRobotToLeftHood = - new Transform3d( - Inches.of(7.268715), Meters.of(0.20792316), Inches.of(16.018516), Rotation3d.kZero); - - public static final Transform3d kRobotToRightHood = - new Transform3d( - Inches.of(-7.270121), - Inches.of(-(12.062888 - (7.5 / 2.0))), - Inches.of(16.018516), - Rotation3d.kZero); - - public static final Transform3d kLeftTurretToLeftHood = - GeomUtil.toPose3d(HoodConstants.kRobotToLeftHood) - .minus( - GeomUtil.toPose3d(TurretConstants.kRobotToLeftTurret) - .plus( - new Transform3d( - Inches.of(7.268715), Inches.of(0), Inches.of(0), new Rotation3d()))); - - public static final Transform3d kRightTurretToRightHood = - GeomUtil.toPose3d(HoodConstants.kRobotToRightHood) - .minus( - GeomUtil.toPose3d(TurretConstants.kRobotToRightTurret) - .plus( - new Transform3d( - Inches.of(-7.270121), Inches.of(0), Inches.of(0), new Rotation3d()))); - - public static final double kMinAngleRad = Units.degreesToRadians(0); - public static final double kMaxAngleRad = Units.degreesToRadians(40); - } - - public static final class FlywheelConstants { - public static final double kGearRatio = 300; - public static final double kSpeedTolerance = 25.0; - - public static final int kLeftFlywheelID = 2; - - public static final Slot0Configs kGains = new Slot0Configs().withKP(1).withKD(0).withKS(0); - public static final MotorOutputConfigs kOutputConfigs = - new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Coast) - .withInverted(InvertedValue.Clockwise_Positive); - } - } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 323cba9..cdecd54 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,10 +11,10 @@ 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.DriveConstants.ModuleConstants; import frc.robot.RobotState.OdometryObservation; import frc.robot.commands.DriveCommands; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.DriveConstants.ModuleConstants; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOPigeon2; import frc.robot.subsystems.drive.ModuleIO; diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 3b7d9f5..d535a65 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -9,7 +9,8 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import frc.robot.Constants.DriveConstants; +import frc.robot.subsystems.drive.DriveConstants; + import org.littletonrobotics.junction.Logger; public class RobotState { diff --git a/src/main/java/frc/robot/RobotVisualizer.java b/src/main/java/frc/robot/RobotVisualizer.java index bc6a90a..b7e21e9 100644 --- a/src/main/java/frc/robot/RobotVisualizer.java +++ b/src/main/java/frc/robot/RobotVisualizer.java @@ -5,8 +5,8 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; -import frc.robot.Constants.ShooterConstants.HoodConstants; -import frc.robot.Constants.ShooterConstants.TurretConstants; +import frc.robot.subsystems.shooter.ShooterConstants.HoodConstants; +import frc.robot.subsystems.shooter.ShooterConstants.TurretConstants; import frc.robot.util.GeomUtil; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 613b470..00a8613 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -20,8 +20,8 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.Constants.DriveConstants; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.DriveConstants; import frc.robot.util.AllianceFlipUtil; import java.text.DecimalFormat; import java.text.NumberFormat; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 1765c09..344dd72 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -26,8 +26,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; -import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.DriveConstants.ModuleConstants; +import frc.robot.subsystems.drive.DriveConstants.ModuleConstants; import frc.robot.Constants.Mode; import frc.robot.RobotState; import frc.robot.RobotState.OdometryObservation; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java new file mode 100644 index 0000000..0b4d0f7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -0,0 +1,315 @@ +package frc.robot.subsystems.drive; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; +import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; +import com.pathplanner.lib.config.ModuleConfig; +import com.pathplanner.lib.config.RobotConfig; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.MomentOfInertia; +import edu.wpi.first.units.measure.Voltage; + +public final class DriveConstants { + public static final SwerveDriveKinematics kSwerveKinematics = + new SwerveDriveKinematics(Drive.getModuleTranslations()); + + public static final double kOdometryFrequency = + ModuleConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0; + public static final double kDriveBaseRadius = + Math.max( + Math.max( + Math.hypot( + ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), + Math.hypot( + ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY)), + Math.max( + Math.hypot(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), + Math.hypot( + ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY))); + + public static final Translation2d[] kModuleTranslations = + new Translation2d[] { + new Translation2d( + ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), + new Translation2d( + ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY), + new Translation2d(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), + new Translation2d( + ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY) + }; + + // TODO: Update for robot + // PathPlanner config constants + public static final double kRobotMassKG = 74.088; + public static final double kRobotMOI = 6.883; + /** Coefficient of friction */ + public static final double kWheelCOF = 1.2; + + public static final RobotConfig kPathplannerConfig = + new RobotConfig( + kRobotMOI, + kRobotMOI, + new ModuleConfig( + ModuleConstants.FrontLeft.WheelRadius, + ModuleConstants.kSpeedAt12Volts.in(MetersPerSecond), + kWheelCOF, + DCMotor.getKrakenX60(1) + .withReduction(ModuleConstants.FrontLeft.DriveMotorGearRatio), + ModuleConstants.FrontLeft.SlipCurrent, + 1), + kModuleTranslations); + + public static final class ModuleConstants { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with + // the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + // TODO: Update for robot + private static final Slot0Configs steerGains = + new Slot0Configs() + .withKP(100) + .withKI(0) + .withKD(0.5) + .withKS(0.1) + .withKV(1.91) + .withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + // TODO: Update for robot + private static final Slot0Configs driveGains = + new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = + ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = + ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = + DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = + SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to + // RemoteCANcoder + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + // TODO: Update for robot + private static final Current kSlipCurrent = Amps.of(120.0); + + // Initial configs for the drive and steer motors and the azimuth encoder; these + // cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API + // documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a + // relatively + // low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true)); + private static final CANcoderConfiguration encoderInitialConfigs = + new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + // TODO: Update for robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + // TODO: Update for robot + private static final double kCoupleRatio = 3.8181818181818183; + // TODO: Update for robot + private static final double kDriveGearRatio = 7.363636363636365; + private static final double kSteerGearRatio = 15.42857142857143; + private static final Distance kWheelRadius = Inches.of(2.167); + // TODO: Update for robot + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + // TODO: Update for robot + private static final int kPigeonId = 1; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = + new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + ConstantCreator = + new SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + // TODO: Update for robot + // Front Left + private static final int kFrontLeftDriveMotorId = 3; + private static final int kFrontLeftSteerMotorId = 2; + private static final int kFrontLeftEncoderId = 1; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(10); + private static final Distance kFrontLeftYPos = Inches.of(10); + // TODO: Update for robot + // Front Right + private static final int kFrontRightDriveMotorId = 1; + private static final int kFrontRightSteerMotorId = 0; + private static final int kFrontRightEncoderId = 0; + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(10); + private static final Distance kFrontRightYPos = Inches.of(-10); + // TODO: Update for robot + // Back Left + private static final int kBackLeftDriveMotorId = 7; + private static final int kBackLeftSteerMotorId = 6; + private static final int kBackLeftEncoderId = 3; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); + private static final boolean kBackLeftSteerMotorInverted = true; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-10); + private static final Distance kBackLeftYPos = Inches.of(10); + // TODO: Update for robot + // Back Right + private static final int kBackRightDriveMotorId = 5; + private static final int kBackRightSteerMotorId = 4; + private static final int kBackRightEncoderId = 2; + private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-10); + private static final Distance kBackRightYPos = Inches.of(-10); + + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, + kFrontLeftDriveMotorId, + kFrontLeftEncoderId, + kFrontLeftEncoderOffset, + kFrontLeftXPos, + kFrontLeftYPos, + kInvertLeftSide, + kFrontLeftSteerMotorInverted, + kFrontLeftEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, + kFrontRightDriveMotorId, + kFrontRightEncoderId, + kFrontRightEncoderOffset, + kFrontRightXPos, + kFrontRightYPos, + kInvertRightSide, + kFrontRightSteerMotorInverted, + kFrontRightEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, + kBackLeftDriveMotorId, + kBackLeftEncoderId, + kBackLeftEncoderOffset, + kBackLeftXPos, + kBackLeftYPos, + kInvertLeftSide, + kBackLeftSteerMotorInverted, + kBackLeftEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, + kBackRightDriveMotorId, + kBackRightEncoderId, + kBackRightEncoderOffset, + kBackRightXPos, + kBackRightYPos, + kInvertRightSide, + kBackRightSteerMotorInverted, + kBackRightEncoderInverted); + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java index 6eba69f..6236486 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -11,7 +11,6 @@ import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -import frc.robot.Constants.DriveConstants; import java.util.Queue; /** IO implementation for NavX. */ diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 9f1fb04..babf892 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -16,8 +16,8 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.DriveConstants.ModuleConstants; +import frc.robot.subsystems.drive.DriveConstants.ModuleConstants; + import java.util.Queue; /** IO implementation for Pigeon 2. */ diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 3d76494..64965a6 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -34,8 +34,8 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.DriveConstants.ModuleConstants; +import frc.robot.subsystems.drive.DriveConstants.ModuleConstants; + import java.util.Queue; /** diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java index 135ba21..d7ba376 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java @@ -32,8 +32,8 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.DriveConstants.ModuleConstants; +import frc.robot.subsystems.drive.DriveConstants.ModuleConstants; + import java.util.Queue; /** diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index b2ce36b..47e4191 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -11,8 +11,8 @@ import com.ctre.phoenix6.StatusSignal; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.RobotController; -import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.DriveConstants.ModuleConstants; +import frc.robot.subsystems.drive.DriveConstants.ModuleConstants; + import java.util.ArrayList; import java.util.List; import java.util.Queue; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java new file mode 100644 index 0000000..1004f2c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -0,0 +1,80 @@ +package frc.robot.subsystems.shooter; + +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; + +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.util.Units; +import frc.robot.util.GeomUtil; + +public final class ShooterConstants { + + public static final class TurretConstants { + public static final double kGearRatio = 10 / 1; + public static final double kMinTurretAngleRad = Units.degreesToRadians(-180); + public static final double kMaxTurretAngleRad = Units.degreesToRadians(180); + + public static final double kLeftMotorId = 12; + public static final double kRightMotorId = 13; + + // +X = Forward, +Y = Left + public static final Transform3d kRobotToLeftTurret = new Transform3d(Inches.of(3.749), Inches.of(8.186), + Inches.of(13.401), Rotation3d.kZero); + + public static final Transform3d kRobotToRightTurret = new Transform3d(Inches.of(3.749), Inches.of(-8.314), + Inches.of(13.401), Rotation3d.kZero); + } + + public static final class HoodConstants { + public static final double kTurretToHoodInches = 1.878; + public static final double kGearRatio = 100 / 1; + + public static final double kLeftHoodID = -1; + public static final double kRightHoodID = -1; + + public static final Transform3d kRobotToLeftHood = new Transform3d( + Inches.of(7.268715), Meters.of(0.20792316), Inches.of(16.018516), Rotation3d.kZero); + + public static final Transform3d kRobotToRightHood = new Transform3d( + Inches.of(-7.270121), + Inches.of(-(12.062888 - (7.5 / 2.0))), + Inches.of(16.018516), + Rotation3d.kZero); + + public static final Transform3d kLeftTurretToLeftHood = GeomUtil.toPose3d(HoodConstants.kRobotToLeftHood) + .minus( + GeomUtil.toPose3d(TurretConstants.kRobotToLeftTurret) + .plus( + new Transform3d( + Inches.of(7.268715), Inches.of(0), Inches.of(0), new Rotation3d()))); + + public static final Transform3d kRightTurretToRightHood = GeomUtil.toPose3d(HoodConstants.kRobotToRightHood) + .minus( + GeomUtil.toPose3d(TurretConstants.kRobotToRightTurret) + .plus( + new Transform3d( + Inches.of(-7.270121), Inches.of(0), Inches.of(0), new Rotation3d()))); + + public static final double kMinAngleRad = Units.degreesToRadians(0); + public static final double kMaxAngleRad = Units.degreesToRadians(40); + } + + public static final class FlywheelConstants { + public static final double kGearRatio = 300; + public static final double kSpeedTolerance = 25.0; + + public static final int kLeftFlywheelID = -1; + public static final int kRightFlywheelID = -1; + + public static final Slot0Configs kGains = new Slot0Configs().withKP(1).withKD(0).withKS(0); + public static final MotorOutputConfigs kOutputConfigs = new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast) + .withInverted(InvertedValue.Clockwise_Positive); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java index cb13ac9..3a8654c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java @@ -8,8 +8,9 @@ public class TrajectoryCalculator { private static final InterpolatingTreeMap shooterTable = new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), ShooterParams::interpolate); + // TODO update values static { - shooterTable.put(1.5, new ShooterParams(2800.0, 35.0)); + shooterTable.put(1.5, new ShooterParams(2800.0, 35.0)); // Meters, RPM, Degrees shooterTable.put(2.0, new ShooterParams(3100.0, 38.0)); shooterTable.put(2.5, new ShooterParams(3400.0, 42.0)); shooterTable.put(3.0, new ShooterParams(3650.0, 46.0)); diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java index 92ef657..9291941 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java @@ -8,8 +8,9 @@ import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.ShooterConstants.FlywheelConstants; import frc.robot.subsystems.shooter.Shooter.ShooterSide; +import frc.robot.subsystems.shooter.ShooterConstants.FlywheelConstants; + import org.littletonrobotics.junction.Logger; public class Flywheel extends SubsystemBase { diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java index 659dbf2..2a9ae7c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java @@ -6,7 +6,7 @@ import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.Constants; -import frc.robot.Constants.ShooterConstants.FlywheelConstants; +import frc.robot.subsystems.shooter.ShooterConstants.FlywheelConstants; public class FlywheelIOSim implements FlywheelIO { private final DCMotor gearbox = DCMotor.getKrakenX44(1); diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java index 5f510a7..91cadea 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -12,7 +12,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import frc.robot.Constants.ShooterConstants.FlywheelConstants; +import frc.robot.subsystems.shooter.ShooterConstants.FlywheelConstants; public class FlywheelIOTalonFX implements FlywheelIO { private final TalonFX motor; diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSim.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSim.java index aa0db37..dc39680 100644 --- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSim.java @@ -6,7 +6,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import frc.robot.Constants; -import frc.robot.Constants.ShooterConstants.HoodConstants; +import frc.robot.subsystems.shooter.ShooterConstants.HoodConstants; public class HoodIOSim implements HoodIO { private final DCMotor gearbox = DCMotor.getNeo550(1); diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java index 15c497b..d62de31 100644 --- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java @@ -1,8 +1,14 @@ package frc.robot.subsystems.shooter.hood; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; + public class HoodIOSparkMax implements HoodIO { + private final SparkMax motor; - public HoodIOSparkMax() {} + public HoodIOSparkMax(int motorID) { + motor = new SparkMax(motorID, MotorType.kBrushless); + } @Override public void updateInputs(HoodIOInputs inputs) { diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java index 7cb11d3..1e59f18 100644 --- a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java @@ -10,9 +10,10 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.ShooterConstants.TurretConstants; import frc.robot.RobotVisualizer; import frc.robot.subsystems.shooter.Shooter.ShooterSide; +import frc.robot.subsystems.shooter.ShooterConstants.TurretConstants; + import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSim.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSim.java index dbc5e74..c89a619 100644 --- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSim.java @@ -7,7 +7,7 @@ import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.Constants; -import frc.robot.Constants.ShooterConstants.TurretConstants; +import frc.robot.subsystems.shooter.ShooterConstants.TurretConstants; public class TurretIOSim implements TurretIO { private final DCMotor gearbox = DCMotor.getNEO(1); diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java index 3369cb3..2531caf 100644 --- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java @@ -18,7 +18,7 @@ import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.Constants.ShooterConstants.TurretConstants; +import frc.robot.subsystems.shooter.ShooterConstants.TurretConstants; import java.util.function.DoubleSupplier; @@ -49,10 +49,10 @@ public TurretIOSparkMax(int motorID) { .feedbackSensor(FeedbackSensor.kPrimaryEncoder); config.softLimit - .reverseSoftLimit(TurretConstants.kMinTurretAngleRad) - .forwardSoftLimit(TurretConstants.kMaxTurretAngleRad) .reverseSoftLimitEnabled(true) - .forwardSoftLimitEnabled(true); + .forwardSoftLimitEnabled(true) + .reverseSoftLimit(TurretConstants.kMinTurretAngleRad) + .forwardSoftLimit(TurretConstants.kMaxTurretAngleRad); config.closedLoop.feedForward .kS(0); diff --git a/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVision.java index db5b2a3..e40dbaf 100644 --- a/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVision.java @@ -7,8 +7,6 @@ package frc.robot.subsystems.vision; -import static frc.robot.Constants.VisionConstants.*; - import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; @@ -84,7 +82,7 @@ public void updateInputs(CameraIOInputs inputs) { var target = result.targets.get(0); // Calculate robot pose - var tagPose = aprilTagLayout.getTagPose(target.fiducialId); + var tagPose = VisionConstants.kAprilTagLayout.getTagPose(target.fiducialId); if (tagPose.isPresent()) { Transform3d fieldToTarget = new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation()); diff --git a/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVisionSim.java index c16900d..9f14e41 100644 --- a/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVisionSim.java +++ b/src/main/java/frc/robot/subsystems/vision/CameraIOPhotonVisionSim.java @@ -7,8 +7,6 @@ package frc.robot.subsystems.vision; -import static frc.robot.Constants.VisionConstants.aprilTagLayout; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform3d; import java.util.function.Supplier; @@ -37,12 +35,12 @@ public CameraIOPhotonVisionSim( // Initialize vision sim if (visionSim == null) { visionSim = new VisionSystemSim("main"); - visionSim.addAprilTags(aprilTagLayout); + visionSim.addAprilTags(VisionConstants.kAprilTagLayout); } // Add sim camera var cameraProperties = new SimCameraProperties(); - cameraSim = new PhotonCameraSim(camera, cameraProperties, aprilTagLayout); + cameraSim = new PhotonCameraSim(camera, cameraProperties, VisionConstants.kAprilTagLayout); visionSim.addCamera(cameraSim, robotToCamera); } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 3d3192a..cc38d14 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -7,8 +7,6 @@ package frc.robot.subsystems.vision; -import static frc.robot.Constants.VisionConstants.*; - import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; @@ -84,7 +82,7 @@ public void periodic() { // Add tag poses for (int tagId : inputs[cameraIndex].tagIds) { - var tagPose = aprilTagLayout.getTagPose(tagId); + var tagPose = VisionConstants.kAprilTagLayout.getTagPose(tagId); if (tagPose.isPresent()) { tagPoses.add(tagPose.get()); } @@ -96,15 +94,15 @@ public void periodic() { boolean rejectPose = observation.tagCount() == 0 // Must have at least one tag || (observation.tagCount() == 1 - && observation.ambiguity() > maxAmbiguity) // Cannot be high ambiguity + && observation.ambiguity() > VisionConstants.kMaxAmbiguity) // Cannot be high ambiguity || Math.abs(observation.pose().getZ()) - > maxZError // Must have realistic Z coordinate + > VisionConstants.kMaxZError // Must have realistic Z coordinate // Must be within the field boundaries || observation.pose().getX() < 0.0 - || observation.pose().getX() > aprilTagLayout.getFieldLength() + || observation.pose().getX() > VisionConstants.kAprilTagLayout.getFieldLength() || observation.pose().getY() < 0.0 - || observation.pose().getY() > aprilTagLayout.getFieldWidth(); + || observation.pose().getY() > VisionConstants.kAprilTagLayout.getFieldWidth(); // Add pose to log robotPoses.add(observation.pose()); @@ -122,15 +120,15 @@ public void periodic() { // Calculate standard deviations double stdDevFactor = Math.pow(observation.averageTagDistance(), 2.0) / observation.tagCount(); - double linearStdDev = linearStdDevBaseline * stdDevFactor; - double angularStdDev = angularStdDevBaseline * stdDevFactor; + double linearStdDev = VisionConstants.kLinearStdDevBaseline * stdDevFactor; + double angularStdDev = VisionConstants.kAngularStdDevBaseline * stdDevFactor; if (observation.type() == PoseObservationType.MEGATAG_2) { - linearStdDev *= linearStdDevMegatag2Factor; - angularStdDev *= angularStdDevMegatag2Factor; + linearStdDev *= VisionConstants.kLinearStdDevMegatag2Factor; + angularStdDev *= VisionConstants.kAngularStdDevMegatag2Factor; } - if (cameraIndex < cameraStdDevFactors.length) { - linearStdDev *= cameraStdDevFactors[cameraIndex]; - angularStdDev *= cameraStdDevFactors[cameraIndex]; + if (cameraIndex < VisionConstants.kCameraStdDevFactors.length) { + linearStdDev *= VisionConstants.kCameraStdDevFactors[cameraIndex]; + angularStdDev *= VisionConstants.kCameraStdDevFactors[cameraIndex]; } // Send vision observation diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java new file mode 100644 index 0000000..d1ff910 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -0,0 +1,45 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; + +public final class VisionConstants { + // AprilTag layout + public static AprilTagFieldLayout kAprilTagLayout = + AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark); + + // Camera names, must match names configured on coprocessor + public static String kCamera0Name = "camera_0"; + public static String kCamera1Name = "camera_1"; + + // Robot to camera transforms + // (Not used by Limelight, configure in web UI instead) + public static Transform3d kRobotToCamera0 = + new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); + public static Transform3d kRobotToCamera1 = + new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); + + // Basic filtering thresholds + public static double kMaxAmbiguity = 0.3; + public static double kMaxZError = 0.75; + + // Standard deviation baselines, for 1 meter distance and 1 tag + // (Adjusted automatically based on distance and # of tags) + public static double kLinearStdDevBaseline = 0.02; // Meters + public static double kAngularStdDevBaseline = 0.06; // Radians + + // Standard deviation multipliers for each camera + // (Adjust to trust some cameras more than others) + public static double[] kCameraStdDevFactors = + new double[] { + 1.0, // Camera 0 + 1.0 // Camera 1 + }; + + // Multipliers to apply for MegaTag 2 observations + public static double kLinearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve + public static double kAngularStdDevMegatag2Factor = + Double.POSITIVE_INFINITY; // No rotation data available + } From 8bdb2130968372eccb8eba215f62e11593f890b1 Mon Sep 17 00:00:00 2001 From: Maxwell Morgan Date: Mon, 16 Feb 2026 15:50:13 -0500 Subject: [PATCH 35/61] Add shoot-on-the-fly --- src/main/java/frc/robot/Constants.java | 1 - src/main/java/frc/robot/RobotContainer.java | 11 +- src/main/java/frc/robot/RobotState.java | 10 +- .../frc/robot/subsystems/drive/Drive.java | 3 +- .../subsystems/drive/DriveConstants.java | 504 +++++++++--------- .../robot/subsystems/drive/GyroIOPigeon2.java | 1 - .../subsystems/drive/ModuleIOTalonFX.java | 1 - .../subsystems/drive/ModuleIOTalonFXS.java | 1 - .../drive/PhoenixOdometryThread.java | 1 - .../frc/robot/subsystems/shooter/Shooter.java | 62 ++- .../subsystems/shooter/ShooterConstants.java | 136 ++--- .../shooter/TrajectoryCalculator.java | 179 ++++++- .../subsystems/shooter/flywheel/Flywheel.java | 1 - .../robot/subsystems/shooter/hood/Hood.java | 18 + .../robot/subsystems/shooter/hood/HoodIO.java | 5 + .../subsystems/shooter/hood/HoodIOSim.java | 43 +- .../shooter/hood/HoodIOSparkMax.java | 60 ++- .../subsystems/shooter/turret/Turret.java | 16 +- .../subsystems/shooter/turret/TurretIO.java | 7 +- .../shooter/turret/TurretIOSim.java | 20 +- .../shooter/turret/TurretIOSparkMax.java | 49 +- .../frc/robot/subsystems/vision/Vision.java | 3 +- .../subsystems/vision/VisionConstants.java | 74 +-- src/main/java/frc/robot/util/HubTracker.java | 188 +++++++ 24 files changed, 958 insertions(+), 436 deletions(-) create mode 100644 src/main/java/frc/robot/util/HubTracker.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index db8e1b6..83a7f82 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -39,5 +39,4 @@ public static enum Mode { public static void disableHAL() { kDisableHAL = true; } - } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cdecd54..96d4b61 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -86,15 +86,22 @@ private void configureBindings() { drive, () -> -driver.getLeftY(), () -> -driver.getLeftX(), () -> -driver.getRightX())); leftShooter.setDefaultCommand( leftShooter.trackTarget( - () -> RobotState.getInstance().getEstimatedPose(), () -> AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d()))); rightShooter.setDefaultCommand( rightShooter.trackTarget( - () -> RobotState.getInstance().getEstimatedPose(), () -> AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d()))); driver .rightBumper() + .whileTrue( + Shooter.shootBothAtTarget( + leftShooter, + rightShooter, + () -> + AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d()))); + + driver + .a() .whileTrue( DriveCommands.joystickDriveAtAngle( drive, diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index d535a65..daaa61c 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -10,7 +10,6 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import frc.robot.subsystems.drive.DriveConstants; - import org.littletonrobotics.junction.Logger; public class RobotState { @@ -110,6 +109,15 @@ public ChassisSpeeds getRobotVelocity() { return robotVelocity; } + /** Get the rotation of the estimated pose. */ + public Rotation2d getRotation() { + return poseEstimator.getEstimatedPosition().getRotation(); + } + + public ChassisSpeeds getFieldVelocity() { + return ChassisSpeeds.fromRobotRelativeSpeeds(robotVelocity, getRotation()); + } + public record OdometryObservation( double timestamp, SwerveModulePosition[] modulePositions, Rotation2d gyroAngle) {} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 344dd72..b9186c0 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -26,10 +26,10 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; -import frc.robot.subsystems.drive.DriveConstants.ModuleConstants; import frc.robot.Constants.Mode; import frc.robot.RobotState; import frc.robot.RobotState.OdometryObservation; +import frc.robot.subsystems.drive.DriveConstants.ModuleConstants; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import org.littletonrobotics.junction.AutoLogOutput; @@ -140,6 +140,7 @@ public void periodic() { RobotState.getInstance() .addOdometryObservation( new OdometryObservation(sampleTimestamps[i], modulePositions, rawGyroRotation)); + RobotState.getInstance().setRobotVelocity(getChassisSpeeds()); } // Update gyro alert diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 0b4d0f7..cdb8d5d 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -23,7 +23,6 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.RobotConfig; - import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.system.plant.DCMotor; @@ -35,281 +34,274 @@ import edu.wpi.first.units.measure.Voltage; public final class DriveConstants { - public static final SwerveDriveKinematics kSwerveKinematics = - new SwerveDriveKinematics(Drive.getModuleTranslations()); + public static final SwerveDriveKinematics kSwerveKinematics = + new SwerveDriveKinematics(Drive.getModuleTranslations()); - public static final double kOdometryFrequency = - ModuleConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0; - public static final double kDriveBaseRadius = - Math.max( - Math.max( - Math.hypot( - ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), - Math.hypot( - ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY)), - Math.max( - Math.hypot(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), - Math.hypot( - ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY))); + public static final double kOdometryFrequency = + ModuleConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0; + public static final double kDriveBaseRadius = + Math.max( + Math.max( + Math.hypot(ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), + Math.hypot( + ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY)), + Math.max( + Math.hypot(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), + Math.hypot( + ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY))); - public static final Translation2d[] kModuleTranslations = - new Translation2d[] { - new Translation2d( - ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), - new Translation2d( - ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY), - new Translation2d(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), - new Translation2d( - ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY) - }; + public static final Translation2d[] kModuleTranslations = + new Translation2d[] { + new Translation2d(ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY), + new Translation2d( + ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY), + new Translation2d(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY), + new Translation2d(ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY) + }; - // TODO: Update for robot - // PathPlanner config constants - public static final double kRobotMassKG = 74.088; - public static final double kRobotMOI = 6.883; - /** Coefficient of friction */ - public static final double kWheelCOF = 1.2; + // TODO: Update for robot + // PathPlanner config constants + public static final double kRobotMassKG = 74.088; + public static final double kRobotMOI = 6.883; + /** Coefficient of friction */ + public static final double kWheelCOF = 1.2; - public static final RobotConfig kPathplannerConfig = - new RobotConfig( - kRobotMOI, - kRobotMOI, - new ModuleConfig( - ModuleConstants.FrontLeft.WheelRadius, - ModuleConstants.kSpeedAt12Volts.in(MetersPerSecond), - kWheelCOF, - DCMotor.getKrakenX60(1) - .withReduction(ModuleConstants.FrontLeft.DriveMotorGearRatio), - ModuleConstants.FrontLeft.SlipCurrent, - 1), - kModuleTranslations); + public static final RobotConfig kPathplannerConfig = + new RobotConfig( + kRobotMOI, + kRobotMOI, + new ModuleConfig( + ModuleConstants.FrontLeft.WheelRadius, + ModuleConstants.kSpeedAt12Volts.in(MetersPerSecond), + kWheelCOF, + DCMotor.getKrakenX60(1).withReduction(ModuleConstants.FrontLeft.DriveMotorGearRatio), + ModuleConstants.FrontLeft.SlipCurrent, + 1), + kModuleTranslations); - public static final class ModuleConstants { - // Both sets of gains need to be tuned to your individual robot. + public static final class ModuleConstants { + // Both sets of gains need to be tuned to your individual robot. - // The steer motor uses any SwerveModule.SteerRequestType control request with - // the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - // TODO: Update for robot - private static final Slot0Configs steerGains = - new Slot0Configs() - .withKP(100) - .withKI(0) - .withKD(0.5) - .withKS(0.1) - .withKV(1.91) - .withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - // TODO: Update for robot - private static final Slot0Configs driveGains = - new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); + // The steer motor uses any SwerveModule.SteerRequestType control request with + // the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + // TODO: Update for robot + private static final Slot0Configs steerGains = + new Slot0Configs() + .withKP(100) + .withKI(0) + .withKD(0.5) + .withKS(0.1) + .withKV(1.91) + .withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + // TODO: Update for robot + private static final Slot0Configs driveGains = + new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = - ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = - ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = - DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = - SteerMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = + DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = + SteerMotorArrangement.TalonFX_Integrated; - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to - // RemoteCANcoder - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to + // RemoteCANcoder + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - // TODO: Update for robot - private static final Current kSlipCurrent = Amps.of(120.0); + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + // TODO: Update for robot + private static final Current kSlipCurrent = Amps.of(120.0); - // Initial configs for the drive and steer motors and the azimuth encoder; these - // cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API - // documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = - new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a - // relatively - // low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true)); - private static final CANcoderConfiguration encoderInitialConfigs = - new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; + // Initial configs for the drive and steer motors and the azimuth encoder; these + // cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API + // documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a + // relatively + // low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true)); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; - // CAN bus that the devices are located on; - // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot"); + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot"); - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - // TODO: Update for robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + // TODO: Update for robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - // TODO: Update for robot - private static final double kCoupleRatio = 3.8181818181818183; - // TODO: Update for robot - private static final double kDriveGearRatio = 7.363636363636365; - private static final double kSteerGearRatio = 15.42857142857143; - private static final Distance kWheelRadius = Inches.of(2.167); - // TODO: Update for robot - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - // TODO: Update for robot - private static final int kPigeonId = 1; + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + // TODO: Update for robot + private static final double kCoupleRatio = 3.8181818181818183; + // TODO: Update for robot + private static final double kDriveGearRatio = 7.363636363636365; + private static final double kSteerGearRatio = 15.42857142857143; + private static final Distance kWheelRadius = Inches.of(2.167); + // TODO: Update for robot + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + // TODO: Update for robot + private static final int kPigeonId = 1; - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - public static final SwerveDrivetrainConstants DrivetrainConstants = - new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); + public static final SwerveDrivetrainConstants DrivetrainConstants = + new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); - private static final SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - ConstantCreator = - new SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); + private static final SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + ConstantCreator = + new SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); - // TODO: Update for robot - // Front Left - private static final int kFrontLeftDriveMotorId = 3; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 1; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); - private static final boolean kFrontLeftSteerMotorInverted = true; - private static final boolean kFrontLeftEncoderInverted = false; + // TODO: Update for robot + // Front Left + private static final int kFrontLeftDriveMotorId = 3; + private static final int kFrontLeftSteerMotorId = 2; + private static final int kFrontLeftEncoderId = 1; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(10); - private static final Distance kFrontLeftYPos = Inches.of(10); - // TODO: Update for robot - // Front Right - private static final int kFrontRightDriveMotorId = 1; - private static final int kFrontRightSteerMotorId = 0; - private static final int kFrontRightEncoderId = 0; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); - private static final boolean kFrontRightSteerMotorInverted = true; - private static final boolean kFrontRightEncoderInverted = false; + private static final Distance kFrontLeftXPos = Inches.of(10); + private static final Distance kFrontLeftYPos = Inches.of(10); + // TODO: Update for robot + // Front Right + private static final int kFrontRightDriveMotorId = 1; + private static final int kFrontRightSteerMotorId = 0; + private static final int kFrontRightEncoderId = 0; + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightEncoderInverted = false; - private static final Distance kFrontRightXPos = Inches.of(10); - private static final Distance kFrontRightYPos = Inches.of(-10); - // TODO: Update for robot - // Back Left - private static final int kBackLeftDriveMotorId = 7; - private static final int kBackLeftSteerMotorId = 6; - private static final int kBackLeftEncoderId = 3; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); - private static final boolean kBackLeftSteerMotorInverted = true; - private static final boolean kBackLeftEncoderInverted = false; + private static final Distance kFrontRightXPos = Inches.of(10); + private static final Distance kFrontRightYPos = Inches.of(-10); + // TODO: Update for robot + // Back Left + private static final int kBackLeftDriveMotorId = 7; + private static final int kBackLeftSteerMotorId = 6; + private static final int kBackLeftEncoderId = 3; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); + private static final boolean kBackLeftSteerMotorInverted = true; + private static final boolean kBackLeftEncoderInverted = false; - private static final Distance kBackLeftXPos = Inches.of(-10); - private static final Distance kBackLeftYPos = Inches.of(10); - // TODO: Update for robot - // Back Right - private static final int kBackRightDriveMotorId = 5; - private static final int kBackRightSteerMotorId = 4; - private static final int kBackRightEncoderId = 2; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); - private static final boolean kBackRightSteerMotorInverted = true; - private static final boolean kBackRightEncoderInverted = false; + private static final Distance kBackLeftXPos = Inches.of(-10); + private static final Distance kBackLeftYPos = Inches.of(10); + // TODO: Update for robot + // Back Right + private static final int kBackRightDriveMotorId = 5; + private static final int kBackRightSteerMotorId = 4; + private static final int kBackRightEncoderId = 2; + private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightEncoderInverted = false; - private static final Distance kBackRightXPos = Inches.of(-10); - private static final Distance kBackRightYPos = Inches.of(-10); + private static final Distance kBackRightXPos = Inches.of(-10); + private static final Distance kBackRightYPos = Inches.of(-10); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, - kFrontLeftDriveMotorId, - kFrontLeftEncoderId, - kFrontLeftEncoderOffset, - kFrontLeftXPos, - kFrontLeftYPos, - kInvertLeftSide, - kFrontLeftSteerMotorInverted, - kFrontLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, - kFrontRightDriveMotorId, - kFrontRightEncoderId, - kFrontRightEncoderOffset, - kFrontRightXPos, - kFrontRightYPos, - kInvertRightSide, - kFrontRightSteerMotorInverted, - kFrontRightEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, - kBackLeftDriveMotorId, - kBackLeftEncoderId, - kBackLeftEncoderOffset, - kBackLeftXPos, - kBackLeftYPos, - kInvertLeftSide, - kBackLeftSteerMotorInverted, - kBackLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, - kBackRightDriveMotorId, - kBackRightEncoderId, - kBackRightEncoderOffset, - kBackRightXPos, - kBackRightYPos, - kInvertRightSide, - kBackRightSteerMotorInverted, - kBackRightEncoderInverted); - } - } \ No newline at end of file + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, + kFrontLeftDriveMotorId, + kFrontLeftEncoderId, + kFrontLeftEncoderOffset, + kFrontLeftXPos, + kFrontLeftYPos, + kInvertLeftSide, + kFrontLeftSteerMotorInverted, + kFrontLeftEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, + kFrontRightDriveMotorId, + kFrontRightEncoderId, + kFrontRightEncoderOffset, + kFrontRightXPos, + kFrontRightYPos, + kInvertRightSide, + kFrontRightSteerMotorInverted, + kFrontRightEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, + kBackLeftDriveMotorId, + kBackLeftEncoderId, + kBackLeftEncoderOffset, + kBackLeftXPos, + kBackLeftYPos, + kInvertLeftSide, + kBackLeftSteerMotorInverted, + kBackLeftEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, + kBackRightDriveMotorId, + kBackRightEncoderId, + kBackRightEncoderOffset, + kBackRightXPos, + kBackRightYPos, + kInvertRightSide, + kBackRightSteerMotorInverted, + kBackRightEncoderInverted); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index babf892..6a8f4ef 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -17,7 +17,6 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.subsystems.drive.DriveConstants.ModuleConstants; - import java.util.Queue; /** IO implementation for Pigeon 2. */ diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 64965a6..a9d861b 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -35,7 +35,6 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; import frc.robot.subsystems.drive.DriveConstants.ModuleConstants; - import java.util.Queue; /** diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java index d7ba376..d2c5412 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java @@ -33,7 +33,6 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; import frc.robot.subsystems.drive.DriveConstants.ModuleConstants; - import java.util.Queue; /** diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 47e4191..3a2b31d 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -12,7 +12,6 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.RobotController; import frc.robot.subsystems.drive.DriveConstants.ModuleConstants; - import java.util.ArrayList; import java.util.List; import java.util.Queue; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index fe2dc6e..3aa0a32 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -4,11 +4,12 @@ package frc.robot.subsystems.shooter; -import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.shooter.TrajectoryCalculator.ShooterCommand; import frc.robot.subsystems.shooter.flywheel.Flywheel; import frc.robot.subsystems.shooter.flywheel.FlywheelIO; import frc.robot.subsystems.shooter.hood.Hood; @@ -39,15 +40,68 @@ public void periodic() { flywheel.periodic(); } - public Command trackTarget( - Supplier robotPoseSupplier, Supplier targetSupplier) { - return Commands.idle(this).alongWith(turret.trackTarget(robotPoseSupplier, targetSupplier)); + /** + * Calculate and apply trajectory parameters for both shooters. + * + * @param leftShooter The left shooter subsystem. + * @param rightShooter The right shooter subsystem. + * @param targetSupplier A supplier for the target. + * @return A RunCommand applying trajectory parameters to both shooters. + */ + public static Command shootBothAtTarget( + Shooter leftShooter, Shooter rightShooter, Supplier targetSupplier) { + return Commands.run( + () -> { + var cmds = TrajectoryCalculator.calculateBoth(targetSupplier.get()); + leftShooter.applyCommand(cmds.left()); + rightShooter.applyCommand(cmds.right()); + }, + leftShooter, + rightShooter); + } + + /** + * Apply a pre-calculated shooter command to this shooter. This does not require the shooter + * subsystem - use when combining with other shooters. + * + * @param cmd The shot parameters to apply. + */ + public void applyCommand(ShooterCommand cmd) { + flywheel.setVelocity(cmd.wheelRPM()); + hood.setAngle(cmd.hoodAngle()); + turret.setPosition(cmd.turretAngle()); + } + + public Command shootAtTarget(Supplier targetSupplier) { + return Commands.run( + () -> { + ShooterCommand cmd = TrajectoryCalculator.calculate(side, targetSupplier.get()); + flywheel.setVelocity(cmd.wheelRPM()); + hood.setAngle(cmd.hoodAngle()); + turret.setPosition(cmd.turretAngle()); + }, + this, + turret, + hood, + flywheel); + } + + public Command trackTarget(Supplier targetSupplier) { + return Commands.idle(this).alongWith(turret.trackTarget(targetSupplier)); } public void setFlywheelVelocity(double velocityRPM) { flywheel.setVelocity(velocityRPM); } + public void setHoodAngle(double angle) { + hood.setAngle(angle); + } + + public void setTurretPosition(Rotation2d position) { + turret.setPosition(position); + } + public ShooterSide getSide() { return side; } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 1004f2c..4bbd00a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -7,74 +7,82 @@ import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; - import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.util.Units; import frc.robot.util.GeomUtil; public final class ShooterConstants { - - public static final class TurretConstants { - public static final double kGearRatio = 10 / 1; - public static final double kMinTurretAngleRad = Units.degreesToRadians(-180); - public static final double kMaxTurretAngleRad = Units.degreesToRadians(180); - - public static final double kLeftMotorId = 12; - public static final double kRightMotorId = 13; - - // +X = Forward, +Y = Left - public static final Transform3d kRobotToLeftTurret = new Transform3d(Inches.of(3.749), Inches.of(8.186), - Inches.of(13.401), Rotation3d.kZero); - - public static final Transform3d kRobotToRightTurret = new Transform3d(Inches.of(3.749), Inches.of(-8.314), - Inches.of(13.401), Rotation3d.kZero); - } - - public static final class HoodConstants { - public static final double kTurretToHoodInches = 1.878; - public static final double kGearRatio = 100 / 1; - - public static final double kLeftHoodID = -1; - public static final double kRightHoodID = -1; - - public static final Transform3d kRobotToLeftHood = new Transform3d( - Inches.of(7.268715), Meters.of(0.20792316), Inches.of(16.018516), Rotation3d.kZero); - - public static final Transform3d kRobotToRightHood = new Transform3d( - Inches.of(-7.270121), - Inches.of(-(12.062888 - (7.5 / 2.0))), - Inches.of(16.018516), - Rotation3d.kZero); - - public static final Transform3d kLeftTurretToLeftHood = GeomUtil.toPose3d(HoodConstants.kRobotToLeftHood) - .minus( - GeomUtil.toPose3d(TurretConstants.kRobotToLeftTurret) - .plus( - new Transform3d( - Inches.of(7.268715), Inches.of(0), Inches.of(0), new Rotation3d()))); - - public static final Transform3d kRightTurretToRightHood = GeomUtil.toPose3d(HoodConstants.kRobotToRightHood) - .minus( - GeomUtil.toPose3d(TurretConstants.kRobotToRightTurret) - .plus( - new Transform3d( - Inches.of(-7.270121), Inches.of(0), Inches.of(0), new Rotation3d()))); - - public static final double kMinAngleRad = Units.degreesToRadians(0); - public static final double kMaxAngleRad = Units.degreesToRadians(40); - } - - public static final class FlywheelConstants { - public static final double kGearRatio = 300; - public static final double kSpeedTolerance = 25.0; - - public static final int kLeftFlywheelID = -1; - public static final int kRightFlywheelID = -1; - - public static final Slot0Configs kGains = new Slot0Configs().withKP(1).withKD(0).withKS(0); - public static final MotorOutputConfigs kOutputConfigs = new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Coast) - .withInverted(InvertedValue.Clockwise_Positive); - } + public static final double kLatencySeconds = 0.05; + + public static final class TurretConstants { + public static final double kGearRatio = 10 / 1; + public static final double kMinTurretAngleRad = Units.degreesToRadians(-180); + public static final double kMaxTurretAngleRad = Units.degreesToRadians(180); + public static final double kAngleTolerance = Units.degreesToRadians(2); + + public static final double kLeftMotorId = 12; + public static final double kRightMotorId = 13; + + // +X = Forward, +Y = Left + public static final Transform3d kRobotToLeftTurret = + new Transform3d(Inches.of(3.749), Inches.of(8.186), Inches.of(13.401), Rotation3d.kZero); + + public static final Transform3d kRobotToRightTurret = + new Transform3d(Inches.of(3.749), Inches.of(-8.314), Inches.of(13.401), Rotation3d.kZero); + } + + public static final class HoodConstants { + public static final double kTurretToHoodInches = 1.878; + public static final double kGearRatio = 100 / 1; + + public static final double kLeftHoodID = -1; + public static final double kRightHoodID = -1; + + public static final double kAngleTolerance = Units.degreesToRadians(5); + + public static final Transform3d kRobotToLeftHood = + new Transform3d( + Inches.of(7.268715), Meters.of(0.20792316), Inches.of(16.018516), Rotation3d.kZero); + + public static final Transform3d kRobotToRightHood = + new Transform3d( + Inches.of(-7.270121), + Inches.of(-(12.062888 - (7.5 / 2.0))), + Inches.of(16.018516), + Rotation3d.kZero); + + public static final Transform3d kLeftTurretToLeftHood = + GeomUtil.toPose3d(HoodConstants.kRobotToLeftHood) + .minus( + GeomUtil.toPose3d(TurretConstants.kRobotToLeftTurret) + .plus( + new Transform3d( + Inches.of(7.268715), Inches.of(0), Inches.of(0), new Rotation3d()))); + + public static final Transform3d kRightTurretToRightHood = + GeomUtil.toPose3d(HoodConstants.kRobotToRightHood) + .minus( + GeomUtil.toPose3d(TurretConstants.kRobotToRightTurret) + .plus( + new Transform3d( + Inches.of(-7.270121), Inches.of(0), Inches.of(0), new Rotation3d()))); + + public static final double kMinAngleRad = Units.degreesToRadians(0); + public static final double kMaxAngleRad = Units.degreesToRadians(40); + } + + public static final class FlywheelConstants { + public static final double kGearRatio = 300; + public static final double kSpeedTolerance = 25.0; + + public static final int kLeftFlywheelID = -1; + public static final int kRightFlywheelID = -1; + + public static final Slot0Configs kGains = new Slot0Configs().withKP(1).withKD(0).withKS(0); + public static final MotorOutputConfigs kOutputConfigs = + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast) + .withInverted(InvertedValue.Clockwise_Positive); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java index 3a8654c..2437db2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java @@ -1,41 +1,178 @@ package frc.robot.subsystems.shooter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.interpolation.InterpolatingTreeMap; import edu.wpi.first.math.interpolation.InverseInterpolator; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import frc.robot.RobotState; +import frc.robot.subsystems.shooter.Shooter.ShooterSide; +import frc.robot.subsystems.shooter.ShooterConstants.TurretConstants; +import frc.robot.util.GeomUtil; +import org.littletonrobotics.junction.Logger; public class TrajectoryCalculator { - private static final InterpolatingTreeMap shooterTable = - new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), ShooterParams::interpolate); - // TODO update values + private static final InterpolatingTreeMap shooterTable = + new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), TrajectoryParams::interpolate); + + private static final double MIN_SHOOTING_DISTANCE = 1.5; + private static final double MAX_SHOOTING_DISTANCE = 5.0; + static { - shooterTable.put(1.5, new ShooterParams(2800.0, 35.0)); // Meters, RPM, Degrees - shooterTable.put(2.0, new ShooterParams(3100.0, 38.0)); - shooterTable.put(2.5, new ShooterParams(3400.0, 42.0)); - shooterTable.put(3.0, new ShooterParams(3650.0, 46.0)); - shooterTable.put(3.5, new ShooterParams(3900.0, 50.0)); - shooterTable.put(4.0, new ShooterParams(4100.0, 54.0)); - shooterTable.put(4.5, new ShooterParams(4350.0, 58.0)); - shooterTable.put(5.0, new ShooterParams(4550.0, 62.0)); + shooterTable.put(1.5, new TrajectoryParams(2800.0, 35.0, 0.38)); + shooterTable.put(2.0, new TrajectoryParams(3100.0, 38.0, 0.45)); + shooterTable.put(2.5, new TrajectoryParams(3400.0, 42.0, 0.52)); + shooterTable.put(3.0, new TrajectoryParams(3650.0, 46.0, 0.60)); + shooterTable.put(3.5, new TrajectoryParams(3900.0, 50.0, 0.68)); + shooterTable.put(4.0, new TrajectoryParams(4100.0, 54.0, 0.76)); + shooterTable.put(4.5, new TrajectoryParams(4350.0, 58.0, 0.85)); + shooterTable.put(5.0, new TrajectoryParams(4550.0, 62.0, 0.94)); + } + + // ========== PUBLIC API ========== + + /** + * Calculate shooter command for a single shooter. Use this when only one shooter needs + * calculation. + */ + public static ShooterCommand calculate(ShooterSide side, Translation2d targetLocation) { + RobotStateData state = getCompensatedRobotState(); + return calculateWithState(side, targetLocation, state); + } + + /** + * Calculate shooter commands for both shooters efficiently. Use this when both shooters need + * calculation - avoids duplicate state queries. + */ + public static DualShooterCommands calculateBoth(Translation2d targetLocation) { + RobotStateData state = getCompensatedRobotState(); + return new DualShooterCommands( + calculateWithState(ShooterSide.LEFT, targetLocation, state), + calculateWithState(ShooterSide.RIGHT, targetLocation, state)); } - // public static ShooterParams calculate(Supplier robotPoseSupplier, - // Supplier robotSpeeds) { + // ========== PRIVATE IMPLEMENTATION ========== + + /** Get and compensate robot state (shared between both shooters). */ + private static RobotStateData getCompensatedRobotState() { + Pose2d robotPose = RobotState.getInstance().getEstimatedPose(); + ChassisSpeeds robotRelativeVel = RobotState.getInstance().getRobotVelocity(); + ChassisSpeeds fieldVel = RobotState.getInstance().getFieldVelocity(); + + Pose2d compensatedRobotPose = + robotPose.exp( + new Twist2d( + robotRelativeVel.vxMetersPerSecond * ShooterConstants.kLatencySeconds, + robotRelativeVel.vyMetersPerSecond * ShooterConstants.kLatencySeconds, + robotRelativeVel.omegaRadiansPerSecond * ShooterConstants.kLatencySeconds)); + + return new RobotStateData(compensatedRobotPose, robotRelativeVel, fieldVel); + } + + /** Calculate shooter command for a specific side using pre-computed robot state. */ + private static ShooterCommand calculateWithState( + ShooterSide side, Translation2d targetLocation, RobotStateData state) { + + // 2. Identify Turret Offset and Position + Transform3d robotToTurret; + switch (side) { + case LEFT: + robotToTurret = TurretConstants.kRobotToLeftTurret; + break; + case RIGHT: + robotToTurret = TurretConstants.kRobotToRightTurret; + default: + robotToTurret = new Transform3d(); + break; + } + + Pose2d turretPose = + state.compensatedRobotPose.transformBy(GeomUtil.toTransform2d(robotToTurret)); - // Pose2d currPose = robotPoseSupplier.get(); - // ChassisSpeeds robotRelativeVel = robotSpeeds.get(); + // 3. Calculate Field-Relative Turret Velocity (Linear + Tangential) + Translation2d tangentialVelRobot = + new Translation2d( + -state.robotRelativeVel.omegaRadiansPerSecond * robotToTurret.getY(), + state.robotRelativeVel.omegaRadiansPerSecond * robotToTurret.getX()); - // } + Translation2d totalTurretVel = + new Translation2d(state.fieldVel.vxMetersPerSecond, state.fieldVel.vyMetersPerSecond) + .plus(tangentialVelRobot.rotateBy(state.compensatedRobotPose.getRotation())); - public record ShooterParams(double wheelRPM, double hoodAngle) - implements Interpolatable { + // 4. Iterative Lookahead + double lookaheadDistance = targetLocation.getDistance(turretPose.getTranslation()); + Translation2d predictedTurretTranslation = turretPose.getTranslation(); + for (int i = 0; i < 10; i++) { + double clampedDistance = + Math.max(MIN_SHOOTING_DISTANCE, Math.min(MAX_SHOOTING_DISTANCE, lookaheadDistance)); + double timeOfFlight = shooterTable.get(clampedDistance).timeOfFlight(); + predictedTurretTranslation = + turretPose.getTranslation().plus(totalTurretVel.times(timeOfFlight)); + lookaheadDistance = targetLocation.getDistance(predictedTurretTranslation); + } + + // 5. Final Angles and Parameters + Rotation2d turretAngleField = targetLocation.minus(predictedTurretTranslation).getAngle(); + Rotation2d turretAngleRobot = turretAngleField.minus(state.compensatedRobotPose.getRotation()); + + double clampedFinalDistance = + Math.max(MIN_SHOOTING_DISTANCE, Math.min(MAX_SHOOTING_DISTANCE, lookaheadDistance)); + TrajectoryParams params = shooterTable.get(clampedFinalDistance); + + // 6. AdvantageScope Logging + Pose2d lookaheadTurretPose = + new Pose2d(predictedTurretTranslation, state.compensatedRobotPose.getRotation()); + Pose2d lookaheadRobotPose = + lookaheadTurretPose.transformBy(GeomUtil.toTransform2d(robotToTurret).inverse()); + + Logger.recordOutput( + "LaunchCalculator/" + side.getName() + "/LookaheadRobotPose", lookaheadRobotPose); + Logger.recordOutput( + "LaunchCalculator/" + side.getName() + "/ShotVector", + new Pose2d(lookaheadRobotPose.getTranslation(), turretAngleField)); + Logger.recordOutput("LaunchCalculator/" + side.getName() + "/Distance", lookaheadDistance); + Logger.recordOutput( + "LaunchCalculator/" + side.getName() + "/DistanceClamped", clampedFinalDistance); + Logger.recordOutput( + "LaunchCalculator/" + side.getName() + "/IsInRange", + lookaheadDistance >= MIN_SHOOTING_DISTANCE && lookaheadDistance <= MAX_SHOOTING_DISTANCE); + + return new ShooterCommand(params.wheelRPM(), params.hoodAngle(), turretAngleRobot); + } + + // Helper for debug if needed + public double getHorizontalVelocity(double distance) { + double clampedDistance = + Math.max(MIN_SHOOTING_DISTANCE, Math.min(MAX_SHOOTING_DISTANCE, distance)); + TrajectoryParams params = shooterTable.get(clampedDistance); + return clampedDistance / params.timeOfFlight(); + } + + // ========== DATA RECORDS ========== + + /** Robot state data that's shared between both shooters */ + private record RobotStateData( + Pose2d compensatedRobotPose, ChassisSpeeds robotRelativeVel, ChassisSpeeds fieldVel) {} + + /** Shooter commands for both shooters */ + public record DualShooterCommands(ShooterCommand left, ShooterCommand right) {} + + public record TrajectoryParams(double wheelRPM, double hoodAngle, double timeOfFlight) + implements Interpolatable { @Override - public ShooterParams interpolate(ShooterParams other, double t) { - return new ShooterParams( + public TrajectoryParams interpolate(TrajectoryParams other, double t) { + return new TrajectoryParams( wheelRPM + (other.wheelRPM - wheelRPM) * t, - hoodAngle + (other.hoodAngle - hoodAngle) * t); + hoodAngle + (other.hoodAngle - hoodAngle) * t, + timeOfFlight + (other.timeOfFlight - timeOfFlight) * t); } } + + public record ShooterCommand(double wheelRPM, double hoodAngle, Rotation2d turretAngle) {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java index 9291941..c7f13bd 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java @@ -10,7 +10,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.shooter.Shooter.ShooterSide; import frc.robot.subsystems.shooter.ShooterConstants.FlywheelConstants; - import org.littletonrobotics.junction.Logger; public class Flywheel extends SubsystemBase { diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java b/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java index 63a54cf..69508a6 100644 --- a/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java +++ b/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java @@ -4,9 +4,12 @@ package frc.robot.subsystems.shooter.hood; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotVisualizer; import frc.robot.subsystems.shooter.Shooter.ShooterSide; +import frc.robot.subsystems.shooter.ShooterConstants.HoodConstants; import org.littletonrobotics.junction.Logger; public class Hood extends SubsystemBase { @@ -15,6 +18,9 @@ public class Hood extends SubsystemBase { private final HoodIO io; private final HoodIOInputsAutoLogged inputs = new HoodIOInputsAutoLogged(); + private boolean atGoal = false; + private Debouncer atGoalDebouncer = new Debouncer(0.2, DebounceType.kFalling); + /** Creates a new Hood. */ public Hood(ShooterSide side, HoodIO io) { this.side = side; @@ -33,6 +39,18 @@ public void periodic() { } } + /** + * Sets the hood to the target angle. + * + * @param angle The target angle (in radians). + */ + public void setAngle(double angle) { + atGoal = + atGoalDebouncer.calculate( + Math.abs(angle - inputs.positionRad) < HoodConstants.kAngleTolerance); + io.setAngle(angle); + } + public double getPosition() { return inputs.positionRad; } diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIO.java index b81643b..50422f0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIO.java @@ -20,4 +20,9 @@ public static class HoodIOInputs { * @param angle The angle for the hood to aim at (in radians). */ default void setAngle(double angle) {} + + /** Run turn motor at the specified open loop value. */ + public default void setOpenLoop(double output) {} + + default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSim.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSim.java index dc39680..3ab6374 100644 --- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSim.java @@ -10,33 +10,36 @@ public class HoodIOSim implements HoodIO { private final DCMotor gearbox = DCMotor.getNeo550(1); - - private final SingleJointedArmSim sim = - new SingleJointedArmSim( - gearbox, - HoodConstants.kGearRatio, - 0.025, - Units.inchesToMeters(7), - HoodConstants.kMinAngleRad, - HoodConstants.kMaxAngleRad, - true, - 0); + private final SingleJointedArmSim sim; private final PIDController pid = new PIDController(1.0, 0.0, 0.0, Constants.kLoopPeriodSeconds); private double appliedVolts = 0.0; - public HoodIOSim() {} + public HoodIOSim() { + sim = + new SingleJointedArmSim( + gearbox, + HoodConstants.kGearRatio, + 0.025, + Units.inchesToMeters(7), + HoodConstants.kMinAngleRad, + HoodConstants.kMaxAngleRad, + true, + 0); + } @Override public void updateInputs(HoodIOInputs inputs) { - sim.setInputVoltage(appliedVolts); + double volts = MathUtil.clamp(appliedVolts, -12.0, 12.0); + + sim.setInputVoltage(volts); sim.update(0.02); inputs.connected = true; inputs.positionRad = sim.getAngleRads(); inputs.velocityRadPerSec = sim.getVelocityRadPerSec(); - inputs.appliedVolts = appliedVolts; + inputs.appliedVolts = volts; inputs.currentDrawAmps = sim.getCurrentDrawAmps(); } @@ -44,6 +47,16 @@ public void updateInputs(HoodIOInputs inputs) { public void setAngle(double angle) { angle = MathUtil.clamp(angle, HoodConstants.kMinAngleRad, HoodConstants.kMaxAngleRad); - appliedVolts = MathUtil.clamp(pid.calculate(sim.getAngleRads(), angle), -12.0, 12.0); + appliedVolts = pid.calculate(sim.getAngleRads(), angle); + } + + @Override + public void setOpenLoop(double output) { + appliedVolts = 12.0 * output; + } + + @Override + public void stop() { + appliedVolts = 0.0; } } diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java index d62de31..6e10146 100644 --- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java @@ -1,18 +1,72 @@ package frc.robot.subsystems.shooter.hood; -import com.revrobotics.spark.SparkMax; +import static frc.robot.util.SparkUtil.ifOk; +import static frc.robot.util.SparkUtil.sparkStickyFault; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import frc.robot.subsystems.shooter.ShooterConstants.HoodConstants; +import java.util.function.DoubleSupplier; public class HoodIOSparkMax implements HoodIO { private final SparkMax motor; + private final RelativeEncoder encoder; + private final SparkClosedLoopController motorController; + private final Debouncer connectedDebouncer = new Debouncer(0.5, DebounceType.kFalling); public HoodIOSparkMax(int motorID) { motor = new SparkMax(motorID, MotorType.kBrushless); + encoder = motor.getEncoder(); + motorController = motor.getClosedLoopController(); + + SparkMaxConfig config = new SparkMaxConfig(); + + config.idleMode(IdleMode.kCoast); + + config + .encoder + .positionConversionFactor(2 * Math.PI / HoodConstants.kGearRatio) // No absolute encoder... + .velocityConversionFactor(2 * Math.PI / HoodConstants.kGearRatio / 60.0); + + config.closedLoop.feedForward.kS(0); } @Override public void updateInputs(HoodIOInputs inputs) { - // TODO Auto-generated method stub - HoodIO.super.updateInputs(inputs); + sparkStickyFault = false; + ifOk(motor, encoder::getPosition, (value) -> inputs.positionRad = value); + ifOk(motor, encoder::getVelocity, (value) -> inputs.velocityRadPerSec = value); + ifOk( + motor, + new DoubleSupplier[] {motor::getAppliedOutput, motor::getBusVoltage}, + (values) -> inputs.appliedVolts = values[0] * values[1]); + ifOk(motor, motor::getOutputCurrent, (value) -> inputs.currentDrawAmps = value); + inputs.connected = connectedDebouncer.calculate(!sparkStickyFault); + } + + @Override + public void setAngle(double angle) { + double clampedPosition = + MathUtil.clamp(angle, HoodConstants.kMinAngleRad, HoodConstants.kMaxAngleRad); + + motorController.setSetpoint(clampedPosition, ControlType.kPosition); + } + + @Override + public void setOpenLoop(double output) { + motor.set(MathUtil.clamp(output, -1.0, 1.0)); + } + + @Override + public void stop() { + motor.stopMotor(); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java index 1e59f18..c494e09 100644 --- a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java @@ -4,16 +4,18 @@ package frc.robot.subsystems.shooter.turret; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotState; import frc.robot.RobotVisualizer; import frc.robot.subsystems.shooter.Shooter.ShooterSide; import frc.robot.subsystems.shooter.ShooterConstants.TurretConstants; - import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -25,6 +27,9 @@ public class Turret extends SubsystemBase { private Rotation2d targetAngle = Rotation2d.kZero; + private boolean atGoal = false; + private Debouncer atGoalDebouncer = new Debouncer(0.2, DebounceType.kFalling); + /** Creates a new Turret. */ public Turret(ShooterSide side, TurretIO io) { this.side = side; @@ -45,13 +50,12 @@ public void periodic() { Logger.recordOutput(("Turret/" + side.getName() + "/TargetAngle"), targetAngle); } - public Command trackTarget( - Supplier robotPoseSupplier, Supplier targetSupplier) { + public Command trackTarget(Supplier targetSupplier) { return Commands.run( () -> { Translation2d target = targetSupplier.get(); - Pose2d robotPose = robotPoseSupplier.get(); + Pose2d robotPose = RobotState.getInstance().getEstimatedPose(); Translation2d turretOffset = (this.side == ShooterSide.LEFT @@ -79,6 +83,10 @@ public Command trackTarget( } public void setPosition(Rotation2d position) { + atGoal = + atGoalDebouncer.calculate( + Math.abs(position.getRadians() - inputs.positionRad) < TurretConstants.kAngleTolerance); + io.setPosition(position); } diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java index 1a5c83b..794c731 100644 --- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java @@ -12,8 +12,13 @@ public static class TurretIOInputs { public double positionRad = 0.0; public double velocityRadPerSec = 0.0; public double appliedVolts = 0.0; - public double currentAmps = 0.0; + public double currentDrawAmps = 0.0; } public default void setPosition(Rotation2d position) {} + + /** Run turn motor at the specified open loop value. */ + public default void setOpenLoop(double output) {} + + default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSim.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSim.java index c89a619..668be0e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSim.java @@ -13,7 +13,9 @@ public class TurretIOSim implements TurretIO { private final DCMotor gearbox = DCMotor.getNEO(1); private final DCMotorSim sim; - private PIDController pid = new PIDController(10, 0, 0.3, Constants.kLoopPeriodSeconds); + private PIDController pid = new PIDController(2, 0, 0.3, Constants.kLoopPeriodSeconds); + + private double appliedVolts = 0.0; public TurretIOSim() { pid.reset(); @@ -26,8 +28,7 @@ public TurretIOSim() { @Override public void updateInputs(TurretIOInputs inputs) { - double currentOutput = pid.calculate(sim.getAngularPositionRad()); - double volts = MathUtil.clamp(currentOutput, -12.0, 12.0); + double volts = MathUtil.clamp(appliedVolts, -12.0, 12.0); sim.setInputVoltage(volts); sim.update(0.02); @@ -36,11 +37,22 @@ public void updateInputs(TurretIOInputs inputs) { inputs.positionRad = sim.getAngularPositionRad(); inputs.velocityRadPerSec = sim.getAngularVelocityRadPerSec(); inputs.appliedVolts = volts; - inputs.currentAmps = sim.getCurrentDrawAmps(); + inputs.currentDrawAmps = sim.getCurrentDrawAmps(); } @Override public void setPosition(Rotation2d position) { pid.setSetpoint(position.getRadians()); + appliedVolts = pid.calculate(sim.getAngularPositionRad()); + } + + @Override + public void setOpenLoop(double output) { + appliedVolts = 12.0 * output; + } + + @Override + public void stop() { + appliedVolts = 0.0; } } diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java index 2531caf..083d140 100644 --- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java @@ -7,19 +7,18 @@ import com.revrobotics.PersistMode; import com.revrobotics.RelativeEncoder; import com.revrobotics.ResetMode; -import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.subsystems.shooter.ShooterConstants.TurretConstants; - import java.util.function.DoubleSupplier; public class TurretIOSparkMax implements TurretIO { @@ -36,32 +35,36 @@ public TurretIOSparkMax(int motorID) { SparkMaxConfig config = new SparkMaxConfig(); - config.idleMode(SparkMaxConfig.IdleMode.kBrake); + config.idleMode(IdleMode.kCoast); // .smartCurrentLimit(30); - config.encoder - .positionConversionFactor(2 * Math.PI / TurretConstants.kGearRatio) // No absolute encoder... + config + .encoder + .positionConversionFactor( + 2 * Math.PI / TurretConstants.kGearRatio) // No absolute encoder... .velocityConversionFactor(2 * Math.PI / TurretConstants.kGearRatio / 60.0); - config.closedLoop + config + .closedLoop .pid(2.0, 0.0, 0.1) .positionWrappingEnabled(false) .feedbackSensor(FeedbackSensor.kPrimaryEncoder); - config.softLimit + config + .softLimit .reverseSoftLimitEnabled(true) .forwardSoftLimitEnabled(true) .reverseSoftLimit(TurretConstants.kMinTurretAngleRad) .forwardSoftLimit(TurretConstants.kMaxTurretAngleRad); - config.closedLoop.feedForward - .kS(0); + config.closedLoop.feedForward.kS(0); tryUntilOk( motor, 5, - () -> motor.configure( - config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + () -> + motor.configure( + config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); tryUntilOk(motor, 5, () -> encoder.setPosition(0)); } @@ -72,16 +75,30 @@ public void updateInputs(TurretIOInputs inputs) { ifOk(motor, encoder::getVelocity, (value) -> inputs.velocityRadPerSec = value); ifOk( motor, - new DoubleSupplier[] { motor::getAppliedOutput, motor::getBusVoltage }, + new DoubleSupplier[] {motor::getAppliedOutput, motor::getBusVoltage}, (values) -> inputs.appliedVolts = values[0] * values[1]); - ifOk(motor, motor::getOutputCurrent, (value) -> inputs.currentAmps = value); + ifOk(motor, motor::getOutputCurrent, (value) -> inputs.currentDrawAmps = value); inputs.connected = connectedDebouncer.calculate(!sparkStickyFault); } @Override public void setPosition(Rotation2d position) { - double clampedPosition = MathUtil.clamp(position.getRadians(), TurretConstants.kMinTurretAngleRad, TurretConstants.kMaxTurretAngleRad); + double clampedPosition = + MathUtil.clamp( + position.getRadians(), + TurretConstants.kMinTurretAngleRad, + TurretConstants.kMaxTurretAngleRad); + + motorController.setSetpoint(clampedPosition, ControlType.kPosition); + } - motorController.setSetpoint(clampedPosition, ControlType.kPosition); + @Override + public void setOpenLoop(double output) { + motor.set(MathUtil.clamp(output, -1.0, 1.0)); + } + + @Override + public void stop() { + motor.stopMotor(); } } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index cc38d14..b48db78 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -94,7 +94,8 @@ public void periodic() { boolean rejectPose = observation.tagCount() == 0 // Must have at least one tag || (observation.tagCount() == 1 - && observation.ambiguity() > VisionConstants.kMaxAmbiguity) // Cannot be high ambiguity + && observation.ambiguity() + > VisionConstants.kMaxAmbiguity) // Cannot be high ambiguity || Math.abs(observation.pose().getZ()) > VisionConstants.kMaxZError // Must have realistic Z coordinate diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index d1ff910..82caafe 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -6,40 +6,40 @@ import edu.wpi.first.math.geometry.Transform3d; public final class VisionConstants { - // AprilTag layout - public static AprilTagFieldLayout kAprilTagLayout = - AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark); - - // Camera names, must match names configured on coprocessor - public static String kCamera0Name = "camera_0"; - public static String kCamera1Name = "camera_1"; - - // Robot to camera transforms - // (Not used by Limelight, configure in web UI instead) - public static Transform3d kRobotToCamera0 = - new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); - public static Transform3d kRobotToCamera1 = - new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); - - // Basic filtering thresholds - public static double kMaxAmbiguity = 0.3; - public static double kMaxZError = 0.75; - - // Standard deviation baselines, for 1 meter distance and 1 tag - // (Adjusted automatically based on distance and # of tags) - public static double kLinearStdDevBaseline = 0.02; // Meters - public static double kAngularStdDevBaseline = 0.06; // Radians - - // Standard deviation multipliers for each camera - // (Adjust to trust some cameras more than others) - public static double[] kCameraStdDevFactors = - new double[] { - 1.0, // Camera 0 - 1.0 // Camera 1 - }; - - // Multipliers to apply for MegaTag 2 observations - public static double kLinearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve - public static double kAngularStdDevMegatag2Factor = - Double.POSITIVE_INFINITY; // No rotation data available - } + // AprilTag layout + public static AprilTagFieldLayout kAprilTagLayout = + AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark); + + // Camera names, must match names configured on coprocessor + public static String kCamera0Name = "camera_0"; + public static String kCamera1Name = "camera_1"; + + // Robot to camera transforms + // (Not used by Limelight, configure in web UI instead) + public static Transform3d kRobotToCamera0 = + new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); + public static Transform3d kRobotToCamera1 = + new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); + + // Basic filtering thresholds + public static double kMaxAmbiguity = 0.3; + public static double kMaxZError = 0.75; + + // Standard deviation baselines, for 1 meter distance and 1 tag + // (Adjusted automatically based on distance and # of tags) + public static double kLinearStdDevBaseline = 0.02; // Meters + public static double kAngularStdDevBaseline = 0.06; // Radians + + // Standard deviation multipliers for each camera + // (Adjust to trust some cameras more than others) + public static double[] kCameraStdDevFactors = + new double[] { + 1.0, // Camera 0 + 1.0 // Camera 1 + }; + + // Multipliers to apply for MegaTag 2 observations + public static double kLinearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve + public static double kAngularStdDevMegatag2Factor = + Double.POSITIVE_INFINITY; // No rotation data available +} diff --git a/src/main/java/frc/robot/util/HubTracker.java b/src/main/java/frc/robot/util/HubTracker.java new file mode 100644 index 0000000..b439f3a --- /dev/null +++ b/src/main/java/frc/robot/util/HubTracker.java @@ -0,0 +1,188 @@ +package frc.robot.util; + +import static edu.wpi.first.units.Units.Seconds; + +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import java.util.Optional; + +/** + * Created by FRC Team 5000, Hammerheads. Thank you! + */ +public class HubTracker { + /** + * Returns an {@link Optional} containing the current {@link Shift}. + * Will return {@link Optional#empty()} if disabled or in between auto and teleop. + */ + public static Optional getCurrentShift() { + double matchTime = getMatchTime(); + if (matchTime < 0) return Optional.empty(); + + for (Shift shift : Shift.values()) { + if (matchTime < shift.endTime) { + return Optional.of(shift); + } + } + return Optional.empty(); + } + + /** + * Returns an {@link Optional} containing the current {@link Time} remaining in the current shift. + * Will return {@link Optional#empty()} if disabled or in between auto and teleop. + */ + public static Optional

Values:

+ *
    + *
  • {@link Shift#AUTO}
    • (0-20 sec) + *
  • {@link Shift#TRANSITION}
    • (20-30 sec) + *
  • {@link Shift#SHIFT_1}
    • (30-55 sec) + *
  • {@link Shift#SHIFT_2}
    • (55-80 sec) + *
  • {@link Shift#SHIFT_3}
    • (80-105 sec) + *
  • {@link Shift#SHIFT_4}
    • (105-130 sec) + *
  • {@link Shift#ENDGAME}
    • (130-160 sec) + *
+ */ + public enum Shift { + AUTO(0, 20, ActiveType.BOTH), + TRANSITION(20, 30, ActiveType.BOTH), + SHIFT_1(30, 55, ActiveType.AUTO_LOSER), + SHIFT_2(55, 80, ActiveType.AUTO_WINNER), + SHIFT_3(80, 105, ActiveType.AUTO_LOSER), + SHIFT_4(105, 130, ActiveType.AUTO_WINNER), + ENDGAME(130, 160, ActiveType.BOTH); + + final int startTime; + final int endTime; + final ActiveType activeType; + + private Shift(int startTime, int endTime, ActiveType activeType) { + this.startTime = startTime; + this.endTime = endTime; + this.activeType = activeType; + } + } + + private enum ActiveType { + BOTH, + AUTO_WINNER, + AUTO_LOSER + } +} \ No newline at end of file From fec24cb6cfc6ed41aa0546b5d5591609ce667253 Mon Sep 17 00:00:00 2001 From: Maxwell Morgan Date: Mon, 23 Feb 2026 12:19:22 -0500 Subject: [PATCH 36/61] Clamp the turret angle to be within a 90 degree rotation in both directions --- .vscode/settings.json | 2 +- src/main/java/frc/robot/Constants.java | 3 + src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 77 ++++++++++++++++--- src/main/java/frc/robot/RobotState.java | 13 ++++ .../frc/robot/commands/DriveCommands.java | 29 +++++++ .../frc/robot/subsystems/drive/Drive.java | 2 +- .../frc/robot/subsystems/shooter/Shooter.java | 21 +++++ .../subsystems/shooter/ShooterConstants.java | 6 +- .../subsystems/shooter/flywheel/Flywheel.java | 4 + .../shooter/flywheel/FlywheelIO.java | 3 + .../shooter/flywheel/FlywheelIOSim.java | 7 +- .../shooter/flywheel/FlywheelIOTalonFX.java | 5 ++ .../robot/subsystems/shooter/hood/Hood.java | 8 ++ .../robot/subsystems/shooter/hood/HoodIO.java | 2 +- .../subsystems/shooter/turret/Turret.java | 25 +++++- .../subsystems/shooter/turret/TurretIO.java | 2 +- 17 files changed, 191 insertions(+), 20 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index e981dcf..10020d0 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -70,5 +70,5 @@ "[java]": { "editor.defaultFormatter": "redhat.java" }, - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx8G -Xms100m -Xlog:disable" } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 83a7f82..74c7218 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,6 +7,7 @@ package frc.robot; +import com.pathplanner.lib.config.RobotConfig; import edu.wpi.first.wpilibj.RobotBase; /** @@ -39,4 +40,6 @@ public static enum Mode { public static void disableHAL() { kDisableHAL = true; } + + public static RobotConfig kRobotConfig; } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1fb2f38..7e2b4d0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -75,7 +75,7 @@ public Robot() { @Override public void robotPeriodic() { CachedSupplier.invalidateAll(); - robotContainer.robotPeriodic(); + robotContainer.robotContainerPeriodic(); CommandScheduler.getInstance().run(); RobotVisualizer.getInstance().log("Mechanism3d/Robot"); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 96d4b61..f941a12 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,12 +4,17 @@ package frc.robot; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.RobotState.OdometryObservation; import frc.robot.commands.DriveCommands; @@ -26,6 +31,7 @@ import frc.robot.subsystems.shooter.hood.HoodIOSim; import frc.robot.subsystems.shooter.turret.TurretIOSim; import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.Direction; import frc.robot.util.FieldConstants; import frc.robot.util.FieldConstants.Hub; @@ -77,6 +83,16 @@ public RobotContainer() { break; } + // if (Constants.kCurrentMode == Constants.Mode.REAL) { + // try { + // Constants.kRobotConfig = RobotConfig.fromGUISettings(); + // } catch (Exception e) { + // // Handle exception as needed + // e.printStackTrace(); + // } + // } + + // configurePathPlanner(); configureBindings(); } @@ -91,14 +107,33 @@ private void configureBindings() { rightShooter.trackTarget( () -> AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d()))); - driver - .rightBumper() - .whileTrue( - Shooter.shootBothAtTarget( - leftShooter, - rightShooter, - () -> - AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d()))); + /** + * Driver Bindings: + * + *

LB: Toggle deploy/retract intake LT: Spin intake RB: Shoot LT + A: backspin intake RT: + * Climb RT + A: Unclimb X: reset Gyro D-Pad: CrabWalk LB + RB + Y: Aux Handoff + * + *

Back up Aux Controls: + * + *

Pancake up + down: Pitch of turrets Pancake left + right: rotation of turrets trigger + * button: Fires fuel from turrets + * + *

button 7: deploy intake button 8: run intake button 9: retract intake + * + *

button 6: climber up button 4: climber down + * + *

thumb button: Driver Handoff + */ + driver.povUp().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTH)); + driver.povUpRight().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTHEAST)); + driver.povRight().whileTrue(DriveCommands.crabWalk(drive, Direction.EAST)); + driver.povDownRight().whileTrue(DriveCommands.crabWalk(drive, Direction.SOUTHEAST)); + driver.povDown().whileTrue(DriveCommands.crabWalk(drive, Direction.SOUTH)); + driver.povDownLeft().whileTrue(DriveCommands.crabWalk(drive, Direction.SOUTHWEST)); + driver.povLeft().whileTrue(DriveCommands.crabWalk(drive, Direction.WEST)); + driver.povUpLeft().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTHWEST)); + + driver.rightBumper().whileTrue(Shooter.shootBothAtHub(leftShooter, rightShooter)); driver .a() @@ -126,7 +161,7 @@ private void configureBindings() { () -> Hub.innerCenterPoint.toTranslation2d())); } - public void robotPeriodic() { + public void robotContainerPeriodic() { OdometryObservation obs = new OdometryObservation( Timer.getTimestamp(), drive.getModulePositions(), drive.getRawGyroRotation()); @@ -137,5 +172,27 @@ public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } - public void configureSubsystems() {} + public void configurePathPlanner() { + AutoBuilder.configure( + () -> RobotState.getInstance().getEstimatedPose(), + (pose) -> RobotState.getInstance().setPose(pose), + () -> RobotState.getInstance().getRobotVelocity(), + (speeds, feedforwards) -> drive.runVelocity(speeds), + new PPHolonomicDriveController(new PIDConstants(5, 0, 0), new PIDConstants(0, 0, 0)), + Constants.kRobotConfig, + AllianceFlipUtil::shouldFlip, + drive); + + /** PATHPLANNER COMMANDS */ + NamedCommands.registerCommand( + "resetGyro", + new InstantCommand(() -> RobotState.getInstance().resetRotation(Rotation2d.kZero))); + + NamedCommands.registerCommand( + "scoreBothShooters", + Shooter.shootBothAtTarget( + leftShooter, + rightShooter, + () -> AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d()))); + } } diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index daaa61c..00e1eca 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -82,6 +82,19 @@ public void setPose( poseEstimator.resetPosition(rawGyroRotation, modulePositions, pose); } + /** + * Reset pose estimate and align gyro frame to the given pose. + * + * @param pose The pose to reset the pose estimator to. + */ + public void setPose(Pose2d pose) { + poseEstimator.resetPosition(getRotation(), null, pose); + } + + public void resetRotation(Rotation2d rotation) { + poseEstimator.resetRotation(rotation); + } + /** * Set the robot's velocity * diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 00a8613..a26f9de 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -23,6 +23,7 @@ import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.Direction; import java.text.DecimalFormat; import java.text.NumberFormat; import java.util.LinkedList; @@ -173,6 +174,34 @@ public static Command turnToPoint( .beforeStarting(() -> angleController.reset(drive.getRawGyroRotation().getRadians())); } + public static Command crabWalk(Drive drive, Direction direction) { + return drive.run( + () -> { + double speed = 1.0; // meters per second (tune this) + + // Convert enum to vector + double dx = direction.getDx(); + double dy = direction.getDy(); + + // Normalize so diagonals aren't faster + double magnitude = Math.hypot(dx, dy); + if (magnitude > 0) { + dx /= magnitude; + dy /= magnitude; + } + + // Build chassis speeds (no rotation) + ChassisSpeeds speeds = + new ChassisSpeeds( + dx * speed, // vx (forward) + dy * speed, // vy (left) + 0.0 // omega (no turning) + ); + + drive.runVelocity(speeds); + }); + } + // ----------------------- Characterization Commands ----------------------- /** diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index b9186c0..236f5b7 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -150,7 +150,7 @@ public void periodic() { /** * Runs the drive at the desired velocity. * - * @param speeds Speeds in meters/sec + * @param speeds Robot-relative speeds in meters/sec */ public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 3aa0a32..ff159eb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -16,6 +16,8 @@ import frc.robot.subsystems.shooter.hood.HoodIO; import frc.robot.subsystems.shooter.turret.Turret; import frc.robot.subsystems.shooter.turret.TurretIO; +import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.FieldConstants.Hub; import java.util.function.Supplier; public class Shooter extends SubsystemBase { @@ -40,6 +42,13 @@ public void periodic() { flywheel.periodic(); } + public static Command shootBothAtHub(Shooter leftShooter, Shooter rightShooter) { + return shootBothAtTarget( + leftShooter, + rightShooter, + () -> AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d())); + } + /** * Calculate and apply trajectory parameters for both shooters. * @@ -102,6 +111,18 @@ public void setTurretPosition(Rotation2d position) { turret.setPosition(position); } + public void setFlywheelOpenLoop(double output) { + flywheel.setOpenLoop(output); + } + + public void setHoodOpenLoop(double output) { + hood.setOpenLoop(output); + } + + public void setTurretOpenLoop(double output) { + turret.setOpenLoop(output); + } + public ShooterSide getSide() { return side; } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 4bbd00a..7d82b93 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -17,8 +17,8 @@ public final class ShooterConstants { public static final class TurretConstants { public static final double kGearRatio = 10 / 1; - public static final double kMinTurretAngleRad = Units.degreesToRadians(-180); - public static final double kMaxTurretAngleRad = Units.degreesToRadians(180); + public static final double kMinTurretAngleRad = Units.degreesToRadians(-90); + public static final double kMaxTurretAngleRad = Units.degreesToRadians(90); public static final double kAngleTolerance = Units.degreesToRadians(2); public static final double kLeftMotorId = 12; @@ -69,7 +69,7 @@ public static final class HoodConstants { Inches.of(-7.270121), Inches.of(0), Inches.of(0), new Rotation3d()))); public static final double kMinAngleRad = Units.degreesToRadians(0); - public static final double kMaxAngleRad = Units.degreesToRadians(40); + public static final double kMaxAngleRad = Units.degreesToRadians(30); } public static final class FlywheelConstants { diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java index c7f13bd..1976ded 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java @@ -45,6 +45,10 @@ public void setVelocity(double velocityRPM) { io.setVelocity(velocityRPM / 60.0); } + public void setOpenLoop(double output) { + io.setOpenLoop(output); + } + public void stop() { io.stop(); } diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java index d4265c3..79816e7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java @@ -20,5 +20,8 @@ public static class FlywheelIOInputs { */ default void setVelocity(double velocity) {} + /** Run motor at the specified open loop value. */ + public default void setOpenLoop(double output) {} + default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java index 2a9ae7c..7e8bead 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java @@ -42,8 +42,13 @@ public void setVelocity(double velocity) { pid.setSetpoint(velocity); } + @Override + public void setOpenLoop(double output) { + appliedVolts = 12.0 * output; + } + @Override public void stop() { - pid.setSetpoint(0); + appliedVolts = 0.0; } } diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java index 91cadea..a7299a2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -59,6 +59,11 @@ public void setVelocity(double velocity) { motor.setControl(velocityRequest.withVelocity(velocity)); } + @Override + public void setOpenLoop(double output) { + motor.set(output); + } + @Override public void stop() { motor.stopMotor(); diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java b/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java index 69508a6..bb58518 100644 --- a/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java +++ b/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java @@ -51,6 +51,10 @@ public void setAngle(double angle) { io.setAngle(angle); } + public void setOpenLoop(double output) { + io.setOpenLoop(output); + } + public double getPosition() { return inputs.positionRad; } @@ -59,6 +63,10 @@ public double getVelocity() { return inputs.velocityRadPerSec; } + public boolean atGoal() { + return atGoal; + } + public ShooterSide getSide() { return this.side; } diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIO.java index 50422f0..04701a5 100644 --- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIO.java @@ -21,7 +21,7 @@ public static class HoodIOInputs { */ default void setAngle(double angle) {} - /** Run turn motor at the specified open loop value. */ + /** Run motor at the specified open loop value. */ public default void setOpenLoop(double output) {} default void stop() {} diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java index c494e09..33e84a4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java @@ -4,6 +4,7 @@ package frc.robot.subsystems.shooter.turret; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Pose2d; @@ -77,19 +78,37 @@ public Command trackTarget(Supplier targetSupplier) { this.targetAngle = targetAngle; - io.setPosition(targetAngle); + setPosition(targetAngle); }, this); } + /** + * Set the target angle for the turret. + * + *

This will clamp the angle to be within the maximum and minimum rotation of the turret. + * + * @param position A {@link Rotation2d} object representing the target position of the turret. + */ public void setPosition(Rotation2d position) { atGoal = atGoalDebouncer.calculate( Math.abs(position.getRadians() - inputs.positionRad) < TurretConstants.kAngleTolerance); + position = + Rotation2d.fromRadians( + MathUtil.clamp( + position.getRadians(), + TurretConstants.kMinTurretAngleRad, + TurretConstants.kMaxTurretAngleRad)); + io.setPosition(position); } + public void setOpenLoop(double output) { + io.setOpenLoop(output); + } + public double getPosition() { return inputs.positionRad; } @@ -98,6 +117,10 @@ public double getVelocity() { return inputs.velocityRadPerSec; } + public boolean atGoal() { + return atGoal; + } + public ShooterSide getSide() { return this.side; } diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java index 794c731..a3ad4d2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java @@ -17,7 +17,7 @@ public static class TurretIOInputs { public default void setPosition(Rotation2d position) {} - /** Run turn motor at the specified open loop value. */ + /** Run motor at the specified open loop value. */ public default void setOpenLoop(double output) {} default void stop() {} From 6f363683bf68a0f508c6dc5fce3726c024f239b6 Mon Sep 17 00:00:00 2001 From: Maxwell Morgan Date: Thu, 26 Feb 2026 17:57:34 -0500 Subject: [PATCH 37/61] Create controls folder and base classes --- src/main/java/frc/robot/Constants.java | 4 + src/main/java/frc/robot/RobotContainer.java | 19 +- .../java/frc/robot/control/Configurable.java | 12 + .../frc/robot/control/DefaultControls.java | 38 ++ .../frc/robot/control/DriverController.java | 268 ++++++++++++++ .../frc/robot/control/DriverControls.java | 97 +++++ .../java/frc/robot/control/ZoneControls.java | 11 + .../shooter/turret/TurretIOSparkMax.java | 2 +- src/main/java/frc/robot/util/Direction.java | 98 +++++ src/main/java/frc/robot/util/HubTracker.java | 349 +++++++++--------- src/main/java/frc/robot/util/Zone.java | 145 ++++++++ 11 files changed, 852 insertions(+), 191 deletions(-) create mode 100644 src/main/java/frc/robot/control/Configurable.java create mode 100644 src/main/java/frc/robot/control/DefaultControls.java create mode 100644 src/main/java/frc/robot/control/DriverController.java create mode 100644 src/main/java/frc/robot/control/DriverControls.java create mode 100644 src/main/java/frc/robot/control/ZoneControls.java create mode 100644 src/main/java/frc/robot/util/Direction.java create mode 100644 src/main/java/frc/robot/util/Zone.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 74c7218..3b85590 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -42,4 +42,8 @@ public static void disableHAL() { } public static RobotConfig kRobotConfig; + + public static final class DeviceIDs { + + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f941a12..7b05618 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -107,23 +107,6 @@ private void configureBindings() { rightShooter.trackTarget( () -> AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d()))); - /** - * Driver Bindings: - * - *

LB: Toggle deploy/retract intake LT: Spin intake RB: Shoot LT + A: backspin intake RT: - * Climb RT + A: Unclimb X: reset Gyro D-Pad: CrabWalk LB + RB + Y: Aux Handoff - * - *

Back up Aux Controls: - * - *

Pancake up + down: Pitch of turrets Pancake left + right: rotation of turrets trigger - * button: Fires fuel from turrets - * - *

button 7: deploy intake button 8: run intake button 9: retract intake - * - *

button 6: climber up button 4: climber down - * - *

thumb button: Driver Handoff - */ driver.povUp().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTH)); driver.povUpRight().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTHEAST)); driver.povRight().whileTrue(DriveCommands.crabWalk(drive, Direction.EAST)); @@ -131,7 +114,7 @@ private void configureBindings() { driver.povDown().whileTrue(DriveCommands.crabWalk(drive, Direction.SOUTH)); driver.povDownLeft().whileTrue(DriveCommands.crabWalk(drive, Direction.SOUTHWEST)); driver.povLeft().whileTrue(DriveCommands.crabWalk(drive, Direction.WEST)); - driver.povUpLeft().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTHWEST)); + driver.povUpLeft().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTHWEST)); driver.rightBumper().whileTrue(Shooter.shootBothAtHub(leftShooter, rightShooter)); diff --git a/src/main/java/frc/robot/control/Configurable.java b/src/main/java/frc/robot/control/Configurable.java new file mode 100644 index 0000000..65823c8 --- /dev/null +++ b/src/main/java/frc/robot/control/Configurable.java @@ -0,0 +1,12 @@ +package frc.robot.control; + +/** + * Represents any class that registers a group of bindings or settings + * during robot initialization. + * + *

Call {@link #configure()} once from RobotContainer during initialization. + */ +@FunctionalInterface +public interface Configurable { + void configure(); +} \ No newline at end of file diff --git a/src/main/java/frc/robot/control/DefaultControls.java b/src/main/java/frc/robot/control/DefaultControls.java new file mode 100644 index 0000000..429c07c --- /dev/null +++ b/src/main/java/frc/robot/control/DefaultControls.java @@ -0,0 +1,38 @@ +package frc.robot.control; + +import edu.wpi.first.wpilibj.Joystick; +import frc.robot.commands.DriveCommands; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.shooter.Shooter; + +public class DefaultControls implements Configurable { + + private final DriverController driver; + private final Joystick operator; + private final Drive drive; + private final Shooter leftShooter; + private final Shooter rightShooter; + + /** Creates a new DefaultControls. */ + public DefaultControls( + DriverController driver, + Joystick operator, + Drive drive, + Shooter leftShooter, + Shooter rightShooter) { + this.driver = driver; + this.operator = operator; + this.drive = drive; + this.leftShooter = leftShooter; + this.rightShooter = rightShooter; + } + + /** + * Configure all default commands for the subsystems (e.g. includes joystick driving). + */ + @Override + public void configure() { + drive.setDefaultCommand(DriveCommands.joystickDrive(drive, null, null, null)); + } + +} diff --git a/src/main/java/frc/robot/control/DriverController.java b/src/main/java/frc/robot/control/DriverController.java new file mode 100644 index 0000000..e5810c8 --- /dev/null +++ b/src/main/java/frc/robot/control/DriverController.java @@ -0,0 +1,268 @@ +package frc.robot.control; + +import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +/** + * Abstracts controller input so DriverControls works with any supported + * controller type without caring about the underlying hardware. + */ +public interface DriverController { + + Trigger aCross(); + + Trigger bCircle(); + + Trigger xSquare(); + + Trigger yTriangle(); + + Trigger leftBumper(); + + Trigger rightBumper(); + + Trigger leftTrigger(); + + Trigger rightTrigger(); + + Trigger dPadUp(); + + Trigger dPadUpLeft(); + + Trigger dPadUpRight(); + + Trigger dPadDown(); + + Trigger dPadDownLeft(); + + Trigger dPadDownRight(); + + Trigger dPadLeft(); + + Trigger dPadRight(); + + double getLeftX(); + + double getLeftY(); + + double getRightX(); + + double getRightY(); + + class XboxDriverController implements DriverController { + private final CommandXboxController controller; + + public XboxDriverController(CommandXboxController controller) { + this.controller = controller; + } + + @Override + public Trigger aCross() { + return controller.a(); + } + + @Override + public Trigger bCircle() { + return controller.b(); + } + + @Override + public Trigger xSquare() { + return controller.x(); + } + + @Override + public Trigger yTriangle() { + return controller.y(); + } + + @Override + public Trigger leftBumper() { + return controller.leftBumper(); + } + + @Override + public Trigger rightBumper() { + return controller.rightBumper(); + } + + @Override + public Trigger leftTrigger() { + return controller.leftTrigger(); + } + + @Override + public Trigger rightTrigger() { + return controller.rightTrigger(); + } + + @Override + public Trigger dPadUp() { + return controller.povUp(); + } + + @Override + public Trigger dPadUpLeft() { + return controller.povUpLeft(); + } + + @Override + public Trigger dPadUpRight() { + return controller.povUpRight(); + } + + @Override + public Trigger dPadDown() { + return controller.povDown(); + } + + @Override + public Trigger dPadDownLeft() { + return controller.povDownLeft(); + } + + @Override + public Trigger dPadDownRight() { + return controller.povDownRight(); + } + + @Override + public Trigger dPadLeft() { + return controller.povLeft(); + } + + @Override + public Trigger dPadRight() { + return controller.povRight(); + } + + @Override + public double getLeftX() { + return controller.getLeftX(); + } + + @Override + public double getLeftY() { + return controller.getLeftY(); + } + + @Override + public double getRightX() { + return controller.getRightX(); + } + + @Override + public double getRightY() { + return controller.getRightY(); + } + } + + class PS5DriverController implements DriverController { + private final CommandPS5Controller controller; + + public PS5DriverController(CommandPS5Controller controller) { + this.controller = controller; + } + + @Override + public Trigger aCross() { + return controller.cross(); + } + + @Override + public Trigger bCircle() { + return controller.circle(); + } + + @Override + public Trigger xSquare() { + return controller.square(); + } + + @Override + public Trigger yTriangle() { + return controller.triangle(); + } + + @Override + public Trigger leftBumper() { + return controller.L1(); + } + + @Override + public Trigger rightBumper() { + return controller.R1(); + } + + @Override + public Trigger leftTrigger() { + return controller.L2(); + } + + @Override + public Trigger rightTrigger() { + return controller.R2(); + } + + @Override + public Trigger dPadUp() { + return controller.povUp(); + } + + @Override + public Trigger dPadUpLeft() { + return controller.povUpLeft(); + } + + @Override + public Trigger dPadUpRight() { + return controller.povUpRight(); + } + + @Override + public Trigger dPadDown() { + return controller.povDown(); + } + + @Override + public Trigger dPadDownLeft() { + return controller.povDownLeft(); + } + + @Override + public Trigger dPadDownRight() { + return controller.povDownRight(); + } + + @Override + public Trigger dPadLeft() { + return controller.povLeft(); + } + + @Override + public Trigger dPadRight() { + return controller.povRight(); + } + + @Override + public double getLeftX() { + return controller.getLeftX(); + } + + @Override + public double getLeftY() { + return controller.getLeftY(); + } + + @Override + public double getRightX() { + return controller.getRightX(); + } + + @Override + public double getRightY() { + return controller.getRightY(); + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/control/DriverControls.java b/src/main/java/frc/robot/control/DriverControls.java new file mode 100644 index 0000000..16ea199 --- /dev/null +++ b/src/main/java/frc/robot/control/DriverControls.java @@ -0,0 +1,97 @@ +package frc.robot.control; + +import org.littletonrobotics.junction.AutoLogOutput; + +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.shooter.Shooter; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; + +public class DriverControls implements Configurable { + @AutoLogOutput(key = "Control/DriverControls/mode") + private DriverMode mode = DriverMode.ONE_DRIVER; + + public enum DriverMode { + ONE_DRIVER, + TWO_DRIVERS; + } + + private final DriverController driver; + private final Joystick operator; + private final Drive drive; + private final Shooter leftShooter; + private final Shooter rightShooter; + + public DriverControls( + DriverController driver, + Joystick operator, + Drive drive, + Shooter leftShooter, + Shooter rightShooter) { + this.driver = driver; + this.operator = operator; + this.drive = drive; + this.leftShooter = leftShooter; + this.rightShooter = rightShooter; + } + + @Override + public void configure() { + + // Neutral controls (regardless of whether we are in one or two driver mode) + driver.xSquare() + .onTrue(Shooter.shootBothAtHub(leftShooter, rightShooter)); + + configureOneDriver(); + configureTwoDrivers(); + } + + /* + * Driver Bindings: + * + *

LB: Toggle deploy/retract intake LT: Spin intake RB: Shoot LT + A: + * backspin intake RT: + * Climb RT + A: Unclimb X: reset Gyro D-Pad: CrabWalk LB + RB + Y: Aux Handoff + * + */ + + private void configureOneDriver() { + driver.rightBumper().and(this::isOneDriver) + .onTrue(Shooter.shootBothAtHub(leftShooter, rightShooter)); + } + + /* + *

Back up Operator Controls: + * + *

Pancake up + down: Pitch of turrets Pancake left + right: rotation of + * turrets trigger + * button: Fires fuel from turrets + * + *

button 7: deploy intake button 8: run intake button 9: retract intake + * + *

button 6: climber up button 4: climber down + * + *

thumb button: Driver Handoff + */ + private void configureTwoDrivers() { + + } + + private boolean isOneDriver() { + return mode == DriverMode.ONE_DRIVER; + } + + private boolean isTwoDrivers() { + return mode == DriverMode.TWO_DRIVERS; + } + + public void setMode(DriverMode mode) { + this.mode = mode; + configure(); + } + + public DriverMode getMode() { + return mode; + } +} diff --git a/src/main/java/frc/robot/control/ZoneControls.java b/src/main/java/frc/robot/control/ZoneControls.java new file mode 100644 index 0000000..fdbc875 --- /dev/null +++ b/src/main/java/frc/robot/control/ZoneControls.java @@ -0,0 +1,11 @@ +package frc.robot.control; + +public class ZoneControls implements Configurable { + + @Override + public void configure() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'configure'"); + } + +} diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java index 083d140..29cb3f0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java @@ -19,6 +19,7 @@ import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.subsystems.shooter.ShooterConstants.TurretConstants; + import java.util.function.DoubleSupplier; public class TurretIOSparkMax implements TurretIO { @@ -46,7 +47,6 @@ public TurretIOSparkMax(int motorID) { config .closedLoop - .pid(2.0, 0.0, 0.1) .positionWrappingEnabled(false) .feedbackSensor(FeedbackSensor.kPrimaryEncoder); diff --git a/src/main/java/frc/robot/util/Direction.java b/src/main/java/frc/robot/util/Direction.java new file mode 100644 index 0000000..d7e244c --- /dev/null +++ b/src/main/java/frc/robot/util/Direction.java @@ -0,0 +1,98 @@ +package frc.robot.util; + +import edu.wpi.first.math.geometry.Rotation2d; + +/** Enum representing common compass directions (e.g., North, Northeast, East). */ +public enum Direction { + NORTH(0), + NORTHEAST(45), + EAST(90), + SOUTHEAST(135), + SOUTH(180), + SOUTHWEST(225), + WEST(270), + NORTHWEST(315); + + private final double angleDegrees; + + Direction(double angleDegrees) { + this.angleDegrees = angleDegrees; + } + + public double getAngleDegrees() { + return angleDegrees; + } + + public Rotation2d getRotation2d() { + return Rotation2d.fromDegrees(angleDegrees); + } + + public Direction rotateRight45() { + return values()[(this.ordinal() + 1) % 8]; + } + + public Direction rotateLeft45() { + return values()[(this.ordinal() + 7) % 8]; + } + + public Direction rotateRight90() { + return values()[(this.ordinal() + 2) % 8]; + } + + public Direction rotateLeft90() { + return values()[(this.ordinal() + 6) % 8]; + } + + public Direction opposite() { + return values()[(this.ordinal() + 4) % 8]; + } + + public int getDx() { + switch (this) { + case EAST: + case NORTHEAST: + case SOUTHEAST: + return 1; + case WEST: + case NORTHWEST: + case SOUTHWEST: + return -1; + default: + return 0; + } + } + + public int getDy() { + switch (this) { + case NORTH: + case NORTHEAST: + case NORTHWEST: + return 1; + case SOUTH: + case SOUTHEAST: + case SOUTHWEST: + return -1; + default: + return 0; + } + } + + public boolean isCardinal() { + return this == NORTH || this == SOUTH || this == EAST || this == WEST; + } + + public boolean isDiagonal() { + return !isCardinal(); + } + + public static Direction fromAngle(double angleDegrees) { + angleDegrees = ((angleDegrees % 360) + 360) % 360; // normalize 0–359 + int index = (int) Math.round(angleDegrees / 45.0) % 8; + return values()[index]; + } + + @Override + public String toString() { + return name().charAt(0) + name().substring(1).toLowerCase(); + } +} diff --git a/src/main/java/frc/robot/util/HubTracker.java b/src/main/java/frc/robot/util/HubTracker.java index b439f3a..975c0b0 100644 --- a/src/main/java/frc/robot/util/HubTracker.java +++ b/src/main/java/frc/robot/util/HubTracker.java @@ -7,182 +7,187 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import java.util.Optional; -/** - * Created by FRC Team 5000, Hammerheads. Thank you! - */ +/** Created by FRC Team 5000, Hammerheads. Thank you! */ public class HubTracker { - /** - * Returns an {@link Optional} containing the current {@link Shift}. - * Will return {@link Optional#empty()} if disabled or in between auto and teleop. - */ - public static Optional getCurrentShift() { - double matchTime = getMatchTime(); - if (matchTime < 0) return Optional.empty(); - - for (Shift shift : Shift.values()) { - if (matchTime < shift.endTime) { - return Optional.of(shift); - } - } - return Optional.empty(); - } - - /** - * Returns an {@link Optional} containing the current {@link Time} remaining in the current shift. - * Will return {@link Optional#empty()} if disabled or in between auto and teleop. - */ - public static Optional

Values:

- *
    - *
  • {@link Shift#AUTO}
    • (0-20 sec) - *
  • {@link Shift#TRANSITION}
    • (20-30 sec) - *
  • {@link Shift#SHIFT_1}
    • (30-55 sec) - *
  • {@link Shift#SHIFT_2}
    • (55-80 sec) - *
  • {@link Shift#SHIFT_3}
    • (80-105 sec) - *
  • {@link Shift#SHIFT_4}
    • (105-130 sec) - *
  • {@link Shift#ENDGAME}
    • (130-160 sec) - *
- */ - public enum Shift { - AUTO(0, 20, ActiveType.BOTH), - TRANSITION(20, 30, ActiveType.BOTH), - SHIFT_1(30, 55, ActiveType.AUTO_LOSER), - SHIFT_2(55, 80, ActiveType.AUTO_WINNER), - SHIFT_3(80, 105, ActiveType.AUTO_LOSER), - SHIFT_4(105, 130, ActiveType.AUTO_WINNER), - ENDGAME(130, 160, ActiveType.BOTH); - - final int startTime; - final int endTime; - final ActiveType activeType; - - private Shift(int startTime, int endTime, ActiveType activeType) { - this.startTime = startTime; - this.endTime = endTime; - this.activeType = activeType; - } - } - - private enum ActiveType { - BOTH, - AUTO_WINNER, - AUTO_LOSER + return -1; + } + + /** + * Represents an alliance shift.
+ * + *

Values:

+ * + *
    + *
  • {@link Shift#AUTO} (0-20 sec) + *
  • {@link Shift#TRANSITION} (20-30 sec) + *
  • {@link Shift#SHIFT_1} (30-55 sec) + *
  • {@link Shift#SHIFT_2} (55-80 sec) + *
  • {@link Shift#SHIFT_3} (80-105 sec) + *
  • {@link Shift#SHIFT_4} (105-130 sec) + *
  • {@link Shift#ENDGAME} (130-160 sec) + *
+ */ + public enum Shift { + AUTO(0, 20, ActiveType.BOTH), + TRANSITION(20, 30, ActiveType.BOTH), + SHIFT_1(30, 55, ActiveType.AUTO_LOSER), + SHIFT_2(55, 80, ActiveType.AUTO_WINNER), + SHIFT_3(80, 105, ActiveType.AUTO_LOSER), + SHIFT_4(105, 130, ActiveType.AUTO_WINNER), + ENDGAME(130, 160, ActiveType.BOTH); + + final int startTime; + final int endTime; + final ActiveType activeType; + + private Shift(int startTime, int endTime, ActiveType activeType) { + this.startTime = startTime; + this.endTime = endTime; + this.activeType = activeType; } -} \ No newline at end of file + } + + private enum ActiveType { + BOTH, + AUTO_WINNER, + AUTO_LOSER + } +} diff --git a/src/main/java/frc/robot/util/Zone.java b/src/main/java/frc/robot/util/Zone.java new file mode 100644 index 0000000..cc56fd4 --- /dev/null +++ b/src/main/java/frc/robot/util/Zone.java @@ -0,0 +1,145 @@ +package frc.robot.util; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +import java.util.List; +import java.util.function.Supplier; + +/** + * Represents a 2D zone on the field. Zones can be primitive (circle, rectangle, + * polygon) or composite (union, intersection, difference, complement). + * + * Inspired by Team 4481's zone system. + * + * Example usage: + * 

+ *   Zone trenchZone = new RectangleZone(new Translation2d(1, 1), new Translation2d(4, 3));
+ *   Zone safeZone   = new CircleZone(new Translation2d(5, 5), 1.5);
+ *   Zone combined   = trenchZone.union(safeZone);
+ *   combined.contains(robot::getPose).onTrue(hood.down());
+ */
+public interface Zone {
+
+    /**
+     * Returns a Trigger that is active when the supplied translation is inside this zone.
+     *
+     * @param translation a Supplier providing the current Translation2d to check
+     * @return a Trigger that polls containment
+     */
+    Trigger contains(Supplier translation);
+
+    /** Returns a zone that is the union (A ∪ B) of this zone and another. */
+    default Zone union(Zone other) {
+        return translation -> this.contains(translation).or(other.contains(translation));
+    }
+
+    /** Returns a zone that is the intersection (A ∩ B) of this zone and another. */
+    default Zone intersection(Zone other) {
+        return translation -> this.contains(translation).and(other.contains(translation));
+    }
+
+    /**
+     * Returns a zone representing the difference (A \ B): points in this zone
+     * that are NOT in the other zone.
+     */
+    default Zone difference(Zone other) {
+        return translation -> this.contains(translation).and(other.contains(translation).negate());
+    }
+
+    /** Returns the complement of this zone (points NOT in this zone). */
+    default Zone complement() {
+        return translation -> this.contains(translation).negate();
+    }
+
+    /**
+     * A circular zone defined by a center point and a radius.
+     * A translation is inside if its distance to the center is less than the radius.
+     */
+    class CircleZone implements Zone {
+        private final Translation2d center;
+        private final double radius;
+
+        public CircleZone(Translation2d center, double radius) {
+            this.center = center;
+            this.radius = radius;
+        }
+
+        @Override
+        public Trigger contains(Supplier translation) {
+            return new Trigger(() -> translation.get().getDistance(center) < radius);
+        }
+    }
+
+    /**
+     * An axis-aligned rectangular zone defined by two corner points.
+     * A translation is inside if its x and y coordinates fall within the bounding box.
+     */
+    class RectangleZone implements Zone {
+        private final double minX, maxX, minY, maxY;
+
+        public RectangleZone(Translation2d cornerA, Translation2d cornerB) {
+            this.minX = Math.min(cornerA.getX(), cornerB.getX());
+            this.maxX = Math.max(cornerA.getX(), cornerB.getX());
+            this.minY = Math.min(cornerA.getY(), cornerB.getY());
+            this.maxY = Math.max(cornerA.getY(), cornerB.getY());
+        }
+
+        @Override
+        public Trigger contains(Supplier translation) {
+            return new Trigger(() -> {
+                Translation2d t = translation.get();
+                return t.getX() >= minX && t.getX() <= maxX
+                    && t.getY() >= minY && t.getY() <= maxY;
+            });
+        }
+    }
+
+    /**
+     * A polygonal zone defined by an ordered list of vertices.
+     *
+     * Uses a cross-product (winding) approach: for each edge of the polygon,
+     * the point must be on the same side (left side for CCW winding).
+     * Works correctly for convex polygons. For concave polygons, use a
+     * ray-casting approach instead.
+     */
+    class PolygonZone implements Zone {
+        private final List vertices;
+
+        /**
+         * @param vertices ordered vertices of the polygon (CCW winding for correct results)
+         */
+        public PolygonZone(List vertices) {
+            if (vertices.size() < 3) {
+                throw new IllegalArgumentException("A polygon must have at least 3 vertices.");
+            }
+            this.vertices = List.copyOf(vertices);
+        }
+
+        @Override
+        public Trigger contains(Supplier translation) {
+            return new Trigger(() -> isInsidePolygon(translation.get()));
+        }
+
+        /**
+         * Ray-casting algorithm for point-in-polygon detection.
+         * Works for both convex and concave (simple) polygons.
+         */
+        private boolean isInsidePolygon(Translation2d point) {
+            int n = vertices.size();
+            boolean inside = false;
+            double px = point.getX();
+            double py = point.getY();
+
+            for (int i = 0, j = n - 1; i < n; j = i++) {
+                double xi = vertices.get(i).getX(), yi = vertices.get(i).getY();
+                double xj = vertices.get(j).getX(), yj = vertices.get(j).getY();
+
+                boolean intersects = ((yi > py) != (yj > py))
+                        && (px < (xj - xi) * (py - yi) / (yj - yi) + xi);
+                if (intersects) inside = !inside;
+            }
+            return inside;
+        }
+    }
+}
\ No newline at end of file

From 36a4ea4e52df6e6a637da5e08524186c8090cafb Mon Sep 17 00:00:00 2001
From: Maxwell Morgan 
Date: Thu, 26 Feb 2026 18:02:29 -0500
Subject: [PATCH 38/61] Add IntakeConstants.java, remove operator controls

---
 src/main/java/frc/robot/Constants.java        | 1634 ++++++++---------
 src/main/java/frc/robot/RobotContainer.java   |   58 +-
 .../frc/robot/subsystems/intake/Intake.java   |   15 +-
 .../subsystems/intake/IntakeConstants.java    |   13 +
 .../frc/robot/subsystems/intake/IntakeIO.java |   12 +-
 .../subsystems/intake/IntakeIOHardware.java   |   10 +-
 .../robot/subsystems/intake/IntakeIOSim.java  |   21 +-
 7 files changed, 873 insertions(+), 890 deletions(-)
 create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeConstants.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 962951d..0e0954c 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -53,614 +53,623 @@
 import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.units.measure.MomentOfInertia;
 import edu.wpi.first.units.measure.Voltage;
-import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.RobotBase;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import java.util.Map;
 
 /**
- * This class defines the runtime mode used by AdvantageKit. The mode is always
- * "real" when running
- * on a roboRIO. Change the value of "simMode" to switch between "sim" (physics
- * sim) and "replay"
+ * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running
+ * on a roboRIO. Change the value of "simMode" to switch between "sim" (physics sim) and "replay"
  * (log replay from a file).
  */
 public final class Constants {
-    public static final double kLoopPeriodSeconds = 0.02;
+  public static final double kLoopPeriodSeconds = 0.02;
 
-    public static final Mode kSimMode = Mode.SIM;
-    public static final Mode kCurrentMode = RobotBase.isReal() ? Mode.REAL : kSimMode;
+  public static final Mode kSimMode = Mode.SIM;
+  public static final Mode kCurrentMode = RobotBase.isReal() ? Mode.REAL : kSimMode;
 
-    public static enum Mode {
-        /** Running on a real robot. */
-        REAL,
+  public static enum Mode {
+    /** Running on a real robot. */
+    REAL,
 
-        /** Running a physics simulator. */
-        SIM,
+    /** Running a physics simulator. */
+    SIM,
 
-        /** Replaying from a log file. */
-        REPLAY
-    }
+    /** Replaying from a log file. */
+    REPLAY
+  }
 
-    public static boolean kDisableHAL = false;
+  public static boolean kDisableHAL = false;
 
-    public static void disableHAL() {
-        kDisableHAL = true;
-    }
+  public static void disableHAL() {
+    kDisableHAL = true;
+  }
 
-    public static final class DriveConstants {
+  public static final class DriveConstants {
 
-        public static final class ModuleConfigs {
+    public static final class ModuleConfigs {
 
-            public static record ModuleConfig(
-                    int driveMotorID, int angleMotorID, int canCoderID, Rotation2d angleOffset) {
-            }
+      public static record ModuleConfig(
+          int driveMotorID, int angleMotorID, int canCoderID, Rotation2d angleOffset) {}
 
-            /** Module 0 (front left) configs. */
-            public static final ModuleConfig FrontLeft = new ModuleConfig(1, 2, 19,
-                    Rotation2d.fromDegrees(304.36523 - 180));
+      /** Module 0 (front left) configs. */
+      public static final ModuleConfig FrontLeft =
+          new ModuleConfig(1, 2, 19, Rotation2d.fromDegrees(304.36523 - 180));
 
-            /** Module 1 (front right) configs. */
-            public static final ModuleConfig FrontRight = new ModuleConfig(2, 4, 20, Rotation2d.fromDegrees(206.455));
+      /** Module 1 (front right) configs. */
+      public static final ModuleConfig FrontRight =
+          new ModuleConfig(2, 4, 20, Rotation2d.fromDegrees(206.455));
 
-            /** Module 2 (back left) configs. */
-            public static final ModuleConfig BackLeft = new ModuleConfig(5, 6, 21,
-                    Rotation2d.fromDegrees(35.419922 + 180));
+      /** Module 2 (back left) configs. */
+      public static final ModuleConfig BackLeft =
+          new ModuleConfig(5, 6, 21, Rotation2d.fromDegrees(35.419922 + 180));
 
-            /** Module 3 (back right) configs. */
-            public static final ModuleConfig BackRight = new ModuleConfig(7, 8, 22, Rotation2d.fromDegrees(116.89453));
-        }
+      /** Module 3 (back right) configs. */
+      public static final ModuleConfig BackRight =
+          new ModuleConfig(7, 8, 22, Rotation2d.fromDegrees(116.89453));
+    }
 
-        // TunerConstants doesn't include these constants
-        public static final double kOdometryFrequency = ModuleConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0;
-        public static final double kDriveBaseRadius = Math.max(
-                Math.max(
-                        Math.hypot(
-                                ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY),
-                        Math.hypot(
-                                ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY)),
-                Math.max(
-                        Math.hypot(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY),
-                        Math.hypot(
-                                ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY)));
-
-        public static final Translation2d[] kModuleTranslations = new Translation2d[] {
-                new Translation2d(
-                        ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY),
-                new Translation2d(
-                        ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY),
-                new Translation2d(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY),
-                new Translation2d(
-                        ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY)
+    // TunerConstants doesn't include these constants
+    public static final double kOdometryFrequency =
+        ModuleConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0;
+    public static final double kDriveBaseRadius =
+        Math.max(
+            Math.max(
+                Math.hypot(
+                    ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY),
+                Math.hypot(
+                    ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY)),
+            Math.max(
+                Math.hypot(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY),
+                Math.hypot(
+                    ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY)));
+
+    public static final Translation2d[] kModuleTranslations =
+        new Translation2d[] {
+          new Translation2d(
+              ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY),
+          new Translation2d(
+              ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY),
+          new Translation2d(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY),
+          new Translation2d(
+              ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY)
         };
 
-        // PathPlanner config constants
-        public static final double kRobotMassKG = 74.088;
-        public static final double kRobotMOI = 6.883;
-        /** Coefficient of friction */
-        public static final double kWheelCOF = 1.2;
-
-        public static final RobotConfig kPathplannerConfig = new RobotConfig(
-                kRobotMOI,
-                kRobotMOI,
-                new ModuleConfig(
-                        ModuleConstants.FrontLeft.WheelRadius,
-                        ModuleConstants.kSpeedAt12Volts.in(MetersPerSecond),
-                        kWheelCOF,
-                        DCMotor.getKrakenX60Foc(1)
-                                .withReduction(ModuleConstants.FrontLeft.DriveMotorGearRatio),
-                        ModuleConstants.FrontLeft.SlipCurrent,
-                        1),
-                kModuleTranslations);
-
-        public static final IdleMode kDriveIdleMode = IdleMode.kBrake;
-        public static final IdleMode kAngleIdleMode = IdleMode.kBrake;
-        public static final double kDrivePower = 1;
-        public static final double kAnglePower = .9;
-
-        public static final boolean kInvertGyro = false; // Always ensure Gyro is CCW+ CW-
-
-        // drivetrain constants
-        public static final double kTrackWidth = Units.inchesToMeters(24.75);
-        public static final double kWheelBase = Units.inchesToMeters(24.75);
-        public static final double kWheelDiameter = Units.inchesToMeters(4.0);
-        public static final double kWheelRadius = kWheelDiameter / 2.0;
-        public static final double kWheelCircumference = kWheelDiameter * Math.PI;
-
-        // Swerve kinematics, don't change
-        public static final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics(
-                new Translation2d(kWheelBase / 2.0, kTrackWidth / 2.0), // front left
-                new Translation2d(kWheelBase / 2.0, -kTrackWidth / 2.0), // front right
-                new Translation2d(-kWheelBase / 2.0, kTrackWidth / 2.0), // back left
-                new Translation2d(-kWheelBase / 2.0, -kTrackWidth / 2.0)); // back right
-
-        // gear ratios
-        public static final double kDriveGearRatio = (6.12 / 1.0);
-        public static final double kAngleGearRatio = ((150.0 / 7.0) / 1.0);
-
-        // encoder stuff
-        // meters per rotation
-        public static final double kDriveRevToMeters = kWheelCircumference / (kDriveGearRatio);
-        public static final double kDriveRpmToMetersPerSecond = kDriveRevToMeters / 60;
-
+    // PathPlanner config constants
+    public static final double kRobotMassKG = 74.088;
+    public static final double kRobotMOI = 6.883;
+    /** Coefficient of friction */
+    public static final double kWheelCOF = 1.2;
+
+    public static final RobotConfig kPathplannerConfig =
+        new RobotConfig(
+            kRobotMOI,
+            kRobotMOI,
+            new ModuleConfig(
+                ModuleConstants.FrontLeft.WheelRadius,
+                ModuleConstants.kSpeedAt12Volts.in(MetersPerSecond),
+                kWheelCOF,
+                DCMotor.getKrakenX60Foc(1)
+                    .withReduction(ModuleConstants.FrontLeft.DriveMotorGearRatio),
+                ModuleConstants.FrontLeft.SlipCurrent,
+                1),
+            kModuleTranslations);
+
+    public static final IdleMode kDriveIdleMode = IdleMode.kBrake;
+    public static final IdleMode kAngleIdleMode = IdleMode.kBrake;
+    public static final double kDrivePower = 1;
+    public static final double kAnglePower = .9;
+
+    public static final boolean kInvertGyro = false; // Always ensure Gyro is CCW+ CW-
+
+    // drivetrain constants
+    public static final double kTrackWidth = Units.inchesToMeters(24.75);
+    public static final double kWheelBase = Units.inchesToMeters(24.75);
+    public static final double kWheelDiameter = Units.inchesToMeters(4.0);
+    public static final double kWheelRadius = kWheelDiameter / 2.0;
+    public static final double kWheelCircumference = kWheelDiameter * Math.PI;
+
+    // Swerve kinematics, don't change
+    public static final SwerveDriveKinematics swerveKinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(kWheelBase / 2.0, kTrackWidth / 2.0), // front left
+            new Translation2d(kWheelBase / 2.0, -kTrackWidth / 2.0), // front right
+            new Translation2d(-kWheelBase / 2.0, kTrackWidth / 2.0), // back left
+            new Translation2d(-kWheelBase / 2.0, -kTrackWidth / 2.0)); // back right
+
+    // gear ratios
+    public static final double kDriveGearRatio = (6.12 / 1.0);
+    public static final double kAngleGearRatio = ((150.0 / 7.0) / 1.0);
+
+    // encoder stuff
+    // meters per rotation
+    public static final double kDriveRevToMeters = kWheelCircumference / (kDriveGearRatio);
+    public static final double kDriveRpmToMetersPerSecond = kDriveRevToMeters / 60;
+
+    /** The number of degrees that a single rotation of the turn motor turns the // wheel. */
+    public static final double kDegreesPerTurnRotation = 360 / kAngleGearRatio;
+
+    // motor inverts, check these
+    public static final boolean kAngleMotorInvert = true;
+    public static final InvertedValue kDriveMotorInvert = InvertedValue.CounterClockwise_Positive;
+
+    /* Angle Encoder Invert */
+    public static final boolean kCanCoderInvert = false;
+
+    /* Swerve Current Limiting */
+    public static final int kAngleContinuousCurrentLimit = 20;
+    public static final int kAnglePeakCurrentLimit = 40;
+    public static final double kAnglePeakCurrentDuration = 0.1;
+    public static final boolean kAngleEnableCurrentLimit = true;
+
+    public static final int kDriveSupplyCurrentLimit = 60;
+    public static final boolean kDriveSupplyCurrentLimitEnable = true;
+    public static final int kDriveSupplyCurrentThreshold = 60;
+    public static final double kDriveSupplyTimeThreshold = 0.1;
+
+    public static final boolean kDriveEnableCurrentLimit = true;
+
+    /*
+     * These values are used by the drive falcon to ramp in open loop and closed
+     * loop driving.
+     * We found a small open loop ramp (0.25) helps with tread wear, tipping, etc
+     */
+    public static final double kOpenLoopRamp = 0.25;
+    public static final double kClosedLoopRamp = 0.0;
+
+    /* Angle Motor PID Values */
+    public static final double kAngleKP = 0.015;
+    public static final double kAngleKI = 0;
+    public static final double kAngleKD = 0;
+    public static final double kAngleKF = 0;
+
+    /* Drive Motor PID Values */
+
+    public static final double kDriveKP = 0.01;
+    public static final double kDriveKI = 0.0;
+    public static final double kDriveKD = 0.0;
+
+    public static final double kDriveKS = (0.32 / 12);
+    public static final double kDriveKV = (1.988 / 12);
+    public static final double kDriveKA = (1.0449 / 12);
+
+    /* Swerve Profiling Values */
+    /** Meters per second. */
+    public static final double kPhysicalMaxSpeed = 5.0;
+
+    public static final double kMaxTeleDriveSpeed = 4.5;
+    /** Radians per second. */
+    public static final double kPhysicalMaxAngularSpeed = 2 * 2 * Math.PI;
+    /** Radians per second. */
+    public static final double kMaxTeleAngularSpeed = kPhysicalMaxAngularSpeed / 2;
+
+    public static final double kMaxAngularAccelerationSpeed = 4 / Math.PI;
+    /** Radians per second. */
+    public static final double kMaxTeleAngularAccelerationSpeed = kMaxAngularAccelerationSpeed / 2;
+
+    public static final double kDeadband = 0.08;
+
+    public static final Map kDistances =
+        Map.of(
+            0, 0.0,
+            1, 1.0,
+            2, 2.0,
+            3, 3.0,
+            4, 4.0);
+
+    public static class ModuleConstants {
+      // Both sets of gains need to be tuned to your individual robot.
+
+      // The steer motor uses any SwerveModule.SteerRequestType control request with
+      // the
+      // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
+      private static final Slot0Configs steerGains =
+          new Slot0Configs()
+              .withKP(100)
+              .withKI(0)
+              .withKD(0.5)
+              .withKS(0.1)
+              .withKV(1.91)
+              .withKA(0)
+              .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
+      // When using closed-loop control, the drive motor uses the control
+      // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
+      private static final Slot0Configs driveGains =
+          new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124);
+
+      // The closed-loop output type to use for the steer motors;
+      // This affects the PID/FF gains for the steer motors
+      private static final ClosedLoopOutputType kSteerClosedLoopOutput =
+          ClosedLoopOutputType.Voltage;
+      // The closed-loop output type to use for the drive motors;
+      // This affects the PID/FF gains for the drive motors
+      private static final ClosedLoopOutputType kDriveClosedLoopOutput =
+          ClosedLoopOutputType.Voltage;
+
+      // The type of motor used for the drive motor
+      private static final DriveMotorArrangement kDriveMotorType =
+          DriveMotorArrangement.TalonFX_Integrated;
+      // The type of motor used for the drive motor
+      private static final SteerMotorArrangement kSteerMotorType =
+          SteerMotorArrangement.TalonFX_Integrated;
+
+      // The remote sensor feedback type to use for the steer motors;
+      // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to
+      // RemoteCANcoder
+      private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
+
+      // The stator current at which the wheels start to slip;
+      // This needs to be tuned to your individual robot
+      private static final Current kSlipCurrent = Amps.of(120.0);
+
+      // Initial configs for the drive and steer motors and the azimuth encoder; these
+      // cannot be null.
+      // Some configs will be overwritten; check the `with*InitialConfigs()` API
+      // documentation.
+      private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
+      private static final TalonFXConfiguration steerInitialConfigs =
+          new TalonFXConfiguration()
+              .withCurrentLimits(
+                  new CurrentLimitsConfigs()
+                      // Swerve azimuth does not require much torque output, so we can set a
+                      // relatively
+                      // low
+                      // stator current limit to help avoid brownouts without impacting performance.
+                      .withStatorCurrentLimit(Amps.of(60))
+                      .withStatorCurrentLimitEnable(true));
+      private static final CANcoderConfiguration encoderInitialConfigs =
+          new CANcoderConfiguration();
+      // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
+      private static final Pigeon2Configuration pigeonConfigs = null;
+
+      // CAN bus that the devices are located on;
+      // All swerve devices must share the same CAN bus
+      public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot");
+
+      // Theoretical free speed (m/s) at 12 V applied output;
+      // This needs to be tuned to your individual robot
+      public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69);
+
+      // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
+      // This may need to be tuned to your individual robot
+      private static final double kCoupleRatio = 3.8181818181818183;
+
+      private static final double kDriveGearRatio = 7.363636363636365;
+      private static final double kSteerGearRatio = 15.42857142857143;
+      private static final Distance kWheelRadius = Inches.of(2.167);
+
+      private static final boolean kInvertLeftSide = false;
+      private static final boolean kInvertRightSide = true;
+
+      private static final int kPigeonId = 1;
+
+      // These are only used for simulation
+      private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004);
+      private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025);
+      // Simulated voltage necessary to overcome friction
+      private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
+      private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
+
+      public static final SwerveDrivetrainConstants DrivetrainConstants =
+          new SwerveDrivetrainConstants()
+              .withCANBusName(kCANBus.getName())
+              .withPigeon2Id(kPigeonId)
+              .withPigeon2Configs(pigeonConfigs);
+
+      private static final SwerveModuleConstantsFactory<
+              TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+          ConstantCreator =
+              new SwerveModuleConstantsFactory<
+                      TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>()
+                  .withDriveMotorGearRatio(kDriveGearRatio)
+                  .withSteerMotorGearRatio(kSteerGearRatio)
+                  .withCouplingGearRatio(kCoupleRatio)
+                  .withWheelRadius(kWheelRadius)
+                  .withSteerMotorGains(steerGains)
+                  .withDriveMotorGains(driveGains)
+                  .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput)
+                  .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput)
+                  .withSlipCurrent(kSlipCurrent)
+                  .withSpeedAt12Volts(kSpeedAt12Volts)
+                  .withDriveMotorType(kDriveMotorType)
+                  .withSteerMotorType(kSteerMotorType)
+                  .withFeedbackSource(kSteerFeedbackType)
+                  .withDriveMotorInitialConfigs(driveInitialConfigs)
+                  .withSteerMotorInitialConfigs(steerInitialConfigs)
+                  .withEncoderInitialConfigs(encoderInitialConfigs)
+                  .withSteerInertia(kSteerInertia)
+                  .withDriveInertia(kDriveInertia)
+                  .withSteerFrictionVoltage(kSteerFrictionVoltage)
+                  .withDriveFrictionVoltage(kDriveFrictionVoltage);
+
+      // Front Left
+      private static final int kFrontLeftDriveMotorId = 3;
+      private static final int kFrontLeftSteerMotorId = 2;
+      private static final int kFrontLeftEncoderId = 1;
+      private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375);
+      private static final boolean kFrontLeftSteerMotorInverted = true;
+      private static final boolean kFrontLeftEncoderInverted = false;
+
+      private static final Distance kFrontLeftXPos = Inches.of(10);
+      private static final Distance kFrontLeftYPos = Inches.of(10);
+
+      // Front Right
+      private static final int kFrontRightDriveMotorId = 1;
+      private static final int kFrontRightSteerMotorId = 0;
+      private static final int kFrontRightEncoderId = 0;
+      private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875);
+      private static final boolean kFrontRightSteerMotorInverted = true;
+      private static final boolean kFrontRightEncoderInverted = false;
+
+      private static final Distance kFrontRightXPos = Inches.of(10);
+      private static final Distance kFrontRightYPos = Inches.of(-10);
+
+      // Back Left
+      private static final int kBackLeftDriveMotorId = 7;
+      private static final int kBackLeftSteerMotorId = 6;
+      private static final int kBackLeftEncoderId = 3;
+      private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875);
+      private static final boolean kBackLeftSteerMotorInverted = true;
+      private static final boolean kBackLeftEncoderInverted = false;
+
+      private static final Distance kBackLeftXPos = Inches.of(-10);
+      private static final Distance kBackLeftYPos = Inches.of(10);
+
+      // Back Right
+      private static final int kBackRightDriveMotorId = 5;
+      private static final int kBackRightSteerMotorId = 4;
+      private static final int kBackRightEncoderId = 2;
+      private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125);
+      private static final boolean kBackRightSteerMotorInverted = true;
+      private static final boolean kBackRightEncoderInverted = false;
+
+      private static final Distance kBackRightXPos = Inches.of(-10);
+      private static final Distance kBackRightYPos = Inches.of(-10);
+
+      public static final SwerveModuleConstants<
+              TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+          FrontLeft =
+              ConstantCreator.createModuleConstants(
+                  kFrontLeftSteerMotorId,
+                  kFrontLeftDriveMotorId,
+                  kFrontLeftEncoderId,
+                  kFrontLeftEncoderOffset,
+                  kFrontLeftXPos,
+                  kFrontLeftYPos,
+                  kInvertLeftSide,
+                  kFrontLeftSteerMotorInverted,
+                  kFrontLeftEncoderInverted);
+      public static final SwerveModuleConstants<
+              TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+          FrontRight =
+              ConstantCreator.createModuleConstants(
+                  kFrontRightSteerMotorId,
+                  kFrontRightDriveMotorId,
+                  kFrontRightEncoderId,
+                  kFrontRightEncoderOffset,
+                  kFrontRightXPos,
+                  kFrontRightYPos,
+                  kInvertRightSide,
+                  kFrontRightSteerMotorInverted,
+                  kFrontRightEncoderInverted);
+      public static final SwerveModuleConstants<
+              TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+          BackLeft =
+              ConstantCreator.createModuleConstants(
+                  kBackLeftSteerMotorId,
+                  kBackLeftDriveMotorId,
+                  kBackLeftEncoderId,
+                  kBackLeftEncoderOffset,
+                  kBackLeftXPos,
+                  kBackLeftYPos,
+                  kInvertLeftSide,
+                  kBackLeftSteerMotorInverted,
+                  kBackLeftEncoderInverted);
+      public static final SwerveModuleConstants<
+              TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+          BackRight =
+              ConstantCreator.createModuleConstants(
+                  kBackRightSteerMotorId,
+                  kBackRightDriveMotorId,
+                  kBackRightEncoderId,
+                  kBackRightEncoderOffset,
+                  kBackRightXPos,
+                  kBackRightYPos,
+                  kInvertRightSide,
+                  kBackRightSteerMotorInverted,
+                  kBackRightEncoderInverted);
+
+      /**
+       * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot
+       * program,.
+       */
+      // public static CommandSwerveDrivetrain createDrivetrain() {
+      // return new CommandSwerveDrivetrain(
+      // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight);
+      // }
+
+      /**
+       * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types.
+       */
+      public static class TunerSwerveDrivetrain
+          extends SwerveDrivetrain {
         /**
-         * The number of degrees that a single rotation of the turn motor turns the //
-         * wheel.
+         * Constructs a CTRE SwerveDrivetrain using the specified constants.
+         *
+         * 
This constructs the underlying hardware devices, so users should not construct the
+         * devices themselves. If they need the devices, they can access them through getters in the
+         * classes.
+         *
+         * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
+         * @param modules Constants for each specific module
          */
-        public static final double kDegreesPerTurnRotation = 360 / kAngleGearRatio;
-
-        // motor inverts, check these
-        public static final boolean kAngleMotorInvert = true;
-        public static final InvertedValue kDriveMotorInvert = InvertedValue.CounterClockwise_Positive;
-
-        /* Angle Encoder Invert */
-        public static final boolean kCanCoderInvert = false;
-
-        /* Swerve Current Limiting */
-        public static final int kAngleContinuousCurrentLimit = 20;
-        public static final int kAnglePeakCurrentLimit = 40;
-        public static final double kAnglePeakCurrentDuration = 0.1;
-        public static final boolean kAngleEnableCurrentLimit = true;
-
-        public static final int kDriveSupplyCurrentLimit = 60;
-        public static final boolean kDriveSupplyCurrentLimitEnable = true;
-        public static final int kDriveSupplyCurrentThreshold = 60;
-        public static final double kDriveSupplyTimeThreshold = 0.1;
+        public TunerSwerveDrivetrain(
+            SwerveDrivetrainConstants drivetrainConstants,
+            SwerveModuleConstants... modules) {
+          super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules);
+        }
 
-        public static final boolean kDriveEnableCurrentLimit = true;
+        /**
+         * Constructs a CTRE SwerveDrivetrain using the specified constants.
+         *
+         * 
This constructs the underlying hardware devices, so users should not construct the
+         * devices themselves. If they need the devices, they can access them through getters in the
+         * classes.
+         *
+         * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
+         * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or
+         *     set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0.
+         * @param modules Constants for each specific module
+         */
+        public TunerSwerveDrivetrain(
+            SwerveDrivetrainConstants drivetrainConstants,
+            double odometryUpdateFrequency,
+            SwerveModuleConstants... modules) {
+          super(
+              TalonFX::new,
+              TalonFX::new,
+              CANcoder::new,
+              drivetrainConstants,
+              odometryUpdateFrequency,
+              modules);
+        }
 
-        /*
-         * These values are used by the drive falcon to ramp in open loop and closed
-         * loop driving.
-         * We found a small open loop ramp (0.25) helps with tread wear, tipping, etc
+        /**
+         * Constructs a CTRE SwerveDrivetrain using the specified constants.
+         *
+         * 
This constructs the underlying hardware devices, so users should not construct the
+         * devices themselves. If they need the devices, they can access them through getters in the
+         * classes.
+         *
+         * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
+         * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or
+         *     set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0.
+         * @param odometryStandardDeviation The standard deviation for odometry calculation in the
+         *     form [x, y, theta]áµ€, with units in meters and radians
+         * @param visionStandardDeviation The standard deviation for vision calculation in the form
+         *     [x, y, theta]áµ€, with units in meters and radians
+         * @param modules Constants for each specific module
          */
-        public static final double kOpenLoopRamp = 0.25;
-        public static final double kClosedLoopRamp = 0.0;
-
-        /* Angle Motor PID Values */
-        public static final double kAngleKP = 0.015;
-        public static final double kAngleKI = 0;
-        public static final double kAngleKD = 0;
-        public static final double kAngleKF = 0;
-
-        /* Drive Motor PID Values */
-
-        public static final double kDriveKP = 0.01;
-        public static final double kDriveKI = 0.0;
-        public static final double kDriveKD = 0.0;
-
-        public static final double kDriveKS = (0.32 / 12);
-        public static final double kDriveKV = (1.988 / 12);
-        public static final double kDriveKA = (1.0449 / 12);
-
-        /* Swerve Profiling Values */
-        /** Meters per second. */
-        public static final double kPhysicalMaxSpeed = 5.0;
-
-        public static final double kMaxTeleDriveSpeed = 4.5;
-        /** Radians per second. */
-        public static final double kPhysicalMaxAngularSpeed = 2 * 2 * Math.PI;
-        /** Radians per second. */
-        public static final double kMaxTeleAngularSpeed = kPhysicalMaxAngularSpeed / 2;
-
-        public static final double kMaxAngularAccelerationSpeed = 4 / Math.PI;
-        /** Radians per second. */
-        public static final double kMaxTeleAngularAccelerationSpeed = kMaxAngularAccelerationSpeed / 2;
-
-        public static final double kDeadband = 0.08;
-
-        public static final Map kDistances = Map.of(
-                0, 0.0,
-                1, 1.0,
-                2, 2.0,
-                3, 3.0,
-                4, 4.0);
-
-        public static class ModuleConstants {
-            // Both sets of gains need to be tuned to your individual robot.
-
-            // The steer motor uses any SwerveModule.SteerRequestType control request with
-            // the
-            // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
-            private static final Slot0Configs steerGains = new Slot0Configs()
-                    .withKP(100)
-                    .withKI(0)
-                    .withKD(0.5)
-                    .withKS(0.1)
-                    .withKV(1.91)
-                    .withKA(0)
-                    .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
-            // When using closed-loop control, the drive motor uses the control
-            // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
-            private static final Slot0Configs driveGains = new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0)
-                    .withKV(0.124);
-
-            // The closed-loop output type to use for the steer motors;
-            // This affects the PID/FF gains for the steer motors
-            private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage;
-            // The closed-loop output type to use for the drive motors;
-            // This affects the PID/FF gains for the drive motors
-            private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage;
-
-            // The type of motor used for the drive motor
-            private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated;
-            // The type of motor used for the drive motor
-            private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated;
-
-            // The remote sensor feedback type to use for the steer motors;
-            // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to
-            // RemoteCANcoder
-            private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
-
-            // The stator current at which the wheels start to slip;
-            // This needs to be tuned to your individual robot
-            private static final Current kSlipCurrent = Amps.of(120.0);
-
-            // Initial configs for the drive and steer motors and the azimuth encoder; these
-            // cannot be null.
-            // Some configs will be overwritten; check the `with*InitialConfigs()` API
-            // documentation.
-            private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
-            private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration()
-                    .withCurrentLimits(
-                            new CurrentLimitsConfigs()
-                                    // Swerve azimuth does not require much torque output, so we can set a
-                                    // relatively
-                                    // low
-                                    // stator current limit to help avoid brownouts without impacting performance.
-                                    .withStatorCurrentLimit(Amps.of(60))
-                                    .withStatorCurrentLimitEnable(true));
-            private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
-            // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
-            private static final Pigeon2Configuration pigeonConfigs = null;
-
-            // CAN bus that the devices are located on;
-            // All swerve devices must share the same CAN bus
-            public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot");
-
-            // Theoretical free speed (m/s) at 12 V applied output;
-            // This needs to be tuned to your individual robot
-            public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69);
-
-            // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
-            // This may need to be tuned to your individual robot
-            private static final double kCoupleRatio = 3.8181818181818183;
-
-            private static final double kDriveGearRatio = 7.363636363636365;
-            private static final double kSteerGearRatio = 15.42857142857143;
-            private static final Distance kWheelRadius = Inches.of(2.167);
-
-            private static final boolean kInvertLeftSide = false;
-            private static final boolean kInvertRightSide = true;
-
-            private static final int kPigeonId = 1;
-
-            // These are only used for simulation
-            private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004);
-            private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025);
-            // Simulated voltage necessary to overcome friction
-            private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
-            private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
-
-            public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
-                    .withCANBusName(kCANBus.getName())
-                    .withPigeon2Id(kPigeonId)
-                    .withPigeon2Configs(pigeonConfigs);
-
-            private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory()
-                    .withDriveMotorGearRatio(kDriveGearRatio)
-                    .withSteerMotorGearRatio(kSteerGearRatio)
-                    .withCouplingGearRatio(kCoupleRatio)
-                    .withWheelRadius(kWheelRadius)
-                    .withSteerMotorGains(steerGains)
-                    .withDriveMotorGains(driveGains)
-                    .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput)
-                    .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput)
-                    .withSlipCurrent(kSlipCurrent)
-                    .withSpeedAt12Volts(kSpeedAt12Volts)
-                    .withDriveMotorType(kDriveMotorType)
-                    .withSteerMotorType(kSteerMotorType)
-                    .withFeedbackSource(kSteerFeedbackType)
-                    .withDriveMotorInitialConfigs(driveInitialConfigs)
-                    .withSteerMotorInitialConfigs(steerInitialConfigs)
-                    .withEncoderInitialConfigs(encoderInitialConfigs)
-                    .withSteerInertia(kSteerInertia)
-                    .withDriveInertia(kDriveInertia)
-                    .withSteerFrictionVoltage(kSteerFrictionVoltage)
-                    .withDriveFrictionVoltage(kDriveFrictionVoltage);
-
-            // Front Left
-            private static final int kFrontLeftDriveMotorId = 3;
-            private static final int kFrontLeftSteerMotorId = 2;
-            private static final int kFrontLeftEncoderId = 1;
-            private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375);
-            private static final boolean kFrontLeftSteerMotorInverted = true;
-            private static final boolean kFrontLeftEncoderInverted = false;
-
-            private static final Distance kFrontLeftXPos = Inches.of(10);
-            private static final Distance kFrontLeftYPos = Inches.of(10);
-
-            // Front Right
-            private static final int kFrontRightDriveMotorId = 1;
-            private static final int kFrontRightSteerMotorId = 0;
-            private static final int kFrontRightEncoderId = 0;
-            private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875);
-            private static final boolean kFrontRightSteerMotorInverted = true;
-            private static final boolean kFrontRightEncoderInverted = false;
-
-            private static final Distance kFrontRightXPos = Inches.of(10);
-            private static final Distance kFrontRightYPos = Inches.of(-10);
-
-            // Back Left
-            private static final int kBackLeftDriveMotorId = 7;
-            private static final int kBackLeftSteerMotorId = 6;
-            private static final int kBackLeftEncoderId = 3;
-            private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875);
-            private static final boolean kBackLeftSteerMotorInverted = true;
-            private static final boolean kBackLeftEncoderInverted = false;
-
-            private static final Distance kBackLeftXPos = Inches.of(-10);
-            private static final Distance kBackLeftYPos = Inches.of(10);
-
-            // Back Right
-            private static final int kBackRightDriveMotorId = 5;
-            private static final int kBackRightSteerMotorId = 4;
-            private static final int kBackRightEncoderId = 2;
-            private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125);
-            private static final boolean kBackRightSteerMotorInverted = true;
-            private static final boolean kBackRightEncoderInverted = false;
-
-            private static final Distance kBackRightXPos = Inches.of(-10);
-            private static final Distance kBackRightYPos = Inches.of(-10);
-
-            public static final SwerveModuleConstants FrontLeft = ConstantCreator
-                    .createModuleConstants(
-                            kFrontLeftSteerMotorId,
-                            kFrontLeftDriveMotorId,
-                            kFrontLeftEncoderId,
-                            kFrontLeftEncoderOffset,
-                            kFrontLeftXPos,
-                            kFrontLeftYPos,
-                            kInvertLeftSide,
-                            kFrontLeftSteerMotorInverted,
-                            kFrontLeftEncoderInverted);
-            public static final SwerveModuleConstants FrontRight = ConstantCreator
-                    .createModuleConstants(
-                            kFrontRightSteerMotorId,
-                            kFrontRightDriveMotorId,
-                            kFrontRightEncoderId,
-                            kFrontRightEncoderOffset,
-                            kFrontRightXPos,
-                            kFrontRightYPos,
-                            kInvertRightSide,
-                            kFrontRightSteerMotorInverted,
-                            kFrontRightEncoderInverted);
-            public static final SwerveModuleConstants BackLeft = ConstantCreator
-                    .createModuleConstants(
-                            kBackLeftSteerMotorId,
-                            kBackLeftDriveMotorId,
-                            kBackLeftEncoderId,
-                            kBackLeftEncoderOffset,
-                            kBackLeftXPos,
-                            kBackLeftYPos,
-                            kInvertLeftSide,
-                            kBackLeftSteerMotorInverted,
-                            kBackLeftEncoderInverted);
-            public static final SwerveModuleConstants BackRight = ConstantCreator
-                    .createModuleConstants(
-                            kBackRightSteerMotorId,
-                            kBackRightDriveMotorId,
-                            kBackRightEncoderId,
-                            kBackRightEncoderOffset,
-                            kBackRightXPos,
-                            kBackRightYPos,
-                            kInvertRightSide,
-                            kBackRightSteerMotorInverted,
-                            kBackRightEncoderInverted);
-
-            /**
-             * Creates a CommandSwerveDrivetrain instance. This should only be called once
-             * in your robot
-             * program,.
-             */
-            // public static CommandSwerveDrivetrain createDrivetrain() {
-            // return new CommandSwerveDrivetrain(
-            // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight);
-            // }
-
-            /**
-             * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected
-             * device types.
-             */
-            public static class TunerSwerveDrivetrain
-                    extends SwerveDrivetrain {
-                /**
-                 * Constructs a CTRE SwerveDrivetrain using the specified constants.
-                 *
-                 * 

-                 * This constructs the underlying hardware devices, so users should not
-                 * construct the
-                 * devices themselves. If they need the devices, they can access them through
-                 * getters in the
-                 * classes.
-                 *
-                 * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
-                 * @param modules             Constants for each specific module
-                 */
-                public TunerSwerveDrivetrain(
-                        SwerveDrivetrainConstants drivetrainConstants,
-                        SwerveModuleConstants... modules) {
-                    super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules);
-                }
-
-                /**
-                 * Constructs a CTRE SwerveDrivetrain using the specified constants.
-                 *
-                 * 

-                 * This constructs the underlying hardware devices, so users should not
-                 * construct the
-                 * devices themselves. If they need the devices, they can access them through
-                 * getters in the
-                 * classes.
-                 *
-                 * @param drivetrainConstants     Drivetrain-wide constants for the swerve drive
-                 * @param odometryUpdateFrequency The frequency to run the odometry loop. If
-                 *                                unspecified or
-                 *                                set to 0 Hz, this is 250 Hz on CAN FD, and 100
-                 *                                Hz on CAN 2.0.
-                 * @param modules                 Constants for each specific module
-                 */
-                public TunerSwerveDrivetrain(
-                        SwerveDrivetrainConstants drivetrainConstants,
-                        double odometryUpdateFrequency,
-                        SwerveModuleConstants... modules) {
-                    super(
-                            TalonFX::new,
-                            TalonFX::new,
-                            CANcoder::new,
-                            drivetrainConstants,
-                            odometryUpdateFrequency,
-                            modules);
-                }
-
-                /**
-                 * Constructs a CTRE SwerveDrivetrain using the specified constants.
-                 *
-                 * 

-                 * This constructs the underlying hardware devices, so users should not
-                 * construct the
-                 * devices themselves. If they need the devices, they can access them through
-                 * getters in the
-                 * classes.
-                 *
-                 * @param drivetrainConstants       Drivetrain-wide constants for the swerve
-                 *                                  drive
-                 * @param odometryUpdateFrequency   The frequency to run the odometry loop. If
-                 *                                  unspecified or
-                 *                                  set to 0 Hz, this is 250 Hz on CAN FD, and
-                 *                                  100 Hz on CAN 2.0.
-                 * @param odometryStandardDeviation The standard deviation for odometry
-                 *                                  calculation in the
-                 *                                  form [x, y, theta]áµ€, with units in meters
-                 *                                  and radians
-                 * @param visionStandardDeviation   The standard deviation for vision
-                 *                                  calculation in the form
-                 *                                  [x, y, theta]áµ€, with units in meters and
-                 *                                  radians
-                 * @param modules                   Constants for each specific module
-                 */
-                public TunerSwerveDrivetrain(
-                        SwerveDrivetrainConstants drivetrainConstants,
-                        double odometryUpdateFrequency,
-                        Matrix odometryStandardDeviation,
-                        Matrix visionStandardDeviation,
-                        SwerveModuleConstants... modules) {
-                    super(
-                            TalonFX::new,
-                            TalonFX::new,
-                            CANcoder::new,
-                            drivetrainConstants,
-                            odometryUpdateFrequency,
-                            odometryStandardDeviation,
-                            visionStandardDeviation,
-                            modules);
-                }
-            }
+        public TunerSwerveDrivetrain(
+            SwerveDrivetrainConstants drivetrainConstants,
+            double odometryUpdateFrequency,
+            Matrix odometryStandardDeviation,
+            Matrix visionStandardDeviation,
+            SwerveModuleConstants... modules) {
+          super(
+              TalonFX::new,
+              TalonFX::new,
+              CANcoder::new,
+              drivetrainConstants,
+              odometryUpdateFrequency,
+              odometryStandardDeviation,
+              visionStandardDeviation,
+              modules);
         }
+      }
     }
-
-    public class ModuleConstants {
-        // Both sets of gains need to be tuned to your individual robot.
-
-        // The steer motor uses any SwerveModule.SteerRequestType control request with
-        // the
-        // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
-        private static final Slot0Configs steerGains = new Slot0Configs()
-                .withKP(100)
-                .withKI(0)
-                .withKD(0.5)
-                .withKS(0.1)
-                .withKV(1.91)
-                .withKA(0)
-                .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
-        // When using closed-loop control, the drive motor uses the control
-        // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
-        private static final Slot0Configs driveGains = new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0)
-                .withKV(0.124);
-
-        // The closed-loop output type to use for the steer motors;
-        // This affects the PID/FF gains for the steer motors
-        private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage;
-        // The closed-loop output type to use for the drive motors;
-        // This affects the PID/FF gains for the drive motors
-        private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage;
-
-        // The type of motor used for the drive motor
-        private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated;
-        // The type of motor used for the drive motor
-        private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated;
-
-        // The remote sensor feedback type to use for the steer motors;
-        // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to
-        // RemoteCANcoder
-        private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
-
-        // The stator current at which the wheels start to slip;
-        // This needs to be tuned to your individual robot
-        private static final Current kSlipCurrent = Amps.of(120.0);
-
-        // Initial configs for the drive and steer motors and the azimuth encoder; these
-        // cannot be null.
-        // Some configs will be overwritten; check the `with*InitialConfigs()` API
-        // documentation.
-        private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
-        private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration()
-                .withCurrentLimits(
-                        new CurrentLimitsConfigs()
-                                // Swerve azimuth does not require much torque output, so we can set a
-                                // relatively
-                                // low
-                                // stator current limit to help avoid brownouts without impacting performance.
-                                .withStatorCurrentLimit(Amps.of(60))
-                                .withStatorCurrentLimitEnable(true));
-        private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
-        // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
-        private static final Pigeon2Configuration pigeonConfigs = null;
-
-        // CAN bus that the devices are located on;
-        // All swerve devices must share the same CAN bus
-        public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot");
-
-        // Theoretical free speed (m/s) at 12 V applied output;
-        // This needs to be tuned to your individual robot
-        public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69);
-
-        // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
-        // This may need to be tuned to your individual robot
-        private static final double kCoupleRatio = 3.8181818181818183;
-
-        private static final double kDriveGearRatio = 7.363636363636365;
-        private static final double kSteerGearRatio = 15.42857142857143;
-        private static final Distance kWheelRadius = Inches.of(2.167);
-
-        private static final boolean kInvertLeftSide = false;
-        private static final boolean kInvertRightSide = true;
-
-        private static final int kPigeonId = 1;
-
-        // These are only used for simulation
-        private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004);
-        private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025);
-        // Simulated voltage necessary to overcome friction
-        private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
-        private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
-
-        public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
-                .withCANBusName(kCANBus.getName())
-                .withPigeon2Id(kPigeonId)
-                .withPigeon2Configs(pigeonConfigs);
-
-        private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory()
+  }
+
+  public class ModuleConstants {
+    // Both sets of gains need to be tuned to your individual robot.
+
+    // The steer motor uses any SwerveModule.SteerRequestType control request with
+    // the
+    // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
+    private static final Slot0Configs steerGains =
+        new Slot0Configs()
+            .withKP(100)
+            .withKI(0)
+            .withKD(0.5)
+            .withKS(0.1)
+            .withKV(1.91)
+            .withKA(0)
+            .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
+    // When using closed-loop control, the drive motor uses the control
+    // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
+    private static final Slot0Configs driveGains =
+        new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124);
+
+    // The closed-loop output type to use for the steer motors;
+    // This affects the PID/FF gains for the steer motors
+    private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage;
+    // The closed-loop output type to use for the drive motors;
+    // This affects the PID/FF gains for the drive motors
+    private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage;
+
+    // The type of motor used for the drive motor
+    private static final DriveMotorArrangement kDriveMotorType =
+        DriveMotorArrangement.TalonFX_Integrated;
+    // The type of motor used for the drive motor
+    private static final SteerMotorArrangement kSteerMotorType =
+        SteerMotorArrangement.TalonFX_Integrated;
+
+    // The remote sensor feedback type to use for the steer motors;
+    // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to
+    // RemoteCANcoder
+    private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
+
+    // The stator current at which the wheels start to slip;
+    // This needs to be tuned to your individual robot
+    private static final Current kSlipCurrent = Amps.of(120.0);
+
+    // Initial configs for the drive and steer motors and the azimuth encoder; these
+    // cannot be null.
+    // Some configs will be overwritten; check the `with*InitialConfigs()` API
+    // documentation.
+    private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
+    private static final TalonFXConfiguration steerInitialConfigs =
+        new TalonFXConfiguration()
+            .withCurrentLimits(
+                new CurrentLimitsConfigs()
+                    // Swerve azimuth does not require much torque output, so we can set a
+                    // relatively
+                    // low
+                    // stator current limit to help avoid brownouts without impacting performance.
+                    .withStatorCurrentLimit(Amps.of(60))
+                    .withStatorCurrentLimitEnable(true));
+    private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
+    // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
+    private static final Pigeon2Configuration pigeonConfigs = null;
+
+    // CAN bus that the devices are located on;
+    // All swerve devices must share the same CAN bus
+    public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot");
+
+    // Theoretical free speed (m/s) at 12 V applied output;
+    // This needs to be tuned to your individual robot
+    public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69);
+
+    // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
+    // This may need to be tuned to your individual robot
+    private static final double kCoupleRatio = 3.8181818181818183;
+
+    private static final double kDriveGearRatio = 7.363636363636365;
+    private static final double kSteerGearRatio = 15.42857142857143;
+    private static final Distance kWheelRadius = Inches.of(2.167);
+
+    private static final boolean kInvertLeftSide = false;
+    private static final boolean kInvertRightSide = true;
+
+    private static final int kPigeonId = 1;
+
+    // These are only used for simulation
+    private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004);
+    private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025);
+    // Simulated voltage necessary to overcome friction
+    private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
+    private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
+
+    public static final SwerveDrivetrainConstants DrivetrainConstants =
+        new SwerveDrivetrainConstants()
+            .withCANBusName(kCANBus.getName())
+            .withPigeon2Id(kPigeonId)
+            .withPigeon2Configs(pigeonConfigs);
+
+    private static final SwerveModuleConstantsFactory<
+            TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+        ConstantCreator =
+            new SwerveModuleConstantsFactory<
+                    TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>()
                 .withDriveMotorGearRatio(kDriveGearRatio)
                 .withSteerMotorGearRatio(kSteerGearRatio)
                 .withCouplingGearRatio(kCoupleRatio)
@@ -682,255 +691,228 @@ public class ModuleConstants {
                 .withSteerFrictionVoltage(kSteerFrictionVoltage)
                 .withDriveFrictionVoltage(kDriveFrictionVoltage);
 
-        // Front Left
-        private static final int kFrontLeftDriveMotorId = 3;
-        private static final int kFrontLeftSteerMotorId = 2;
-        private static final int kFrontLeftEncoderId = 1;
-        private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375);
-        private static final boolean kFrontLeftSteerMotorInverted = true;
-        private static final boolean kFrontLeftEncoderInverted = false;
-
-        private static final Distance kFrontLeftXPos = Inches.of(10);
-        private static final Distance kFrontLeftYPos = Inches.of(10);
-
-        // Front Right
-        private static final int kFrontRightDriveMotorId = 1;
-        private static final int kFrontRightSteerMotorId = 0;
-        private static final int kFrontRightEncoderId = 0;
-        private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875);
-        private static final boolean kFrontRightSteerMotorInverted = true;
-        private static final boolean kFrontRightEncoderInverted = false;
-
-        private static final Distance kFrontRightXPos = Inches.of(10);
-        private static final Distance kFrontRightYPos = Inches.of(-10);
-
-        // Back Left
-        private static final int kBackLeftDriveMotorId = 7;
-        private static final int kBackLeftSteerMotorId = 6;
-        private static final int kBackLeftEncoderId = 3;
-        private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875);
-        private static final boolean kBackLeftSteerMotorInverted = true;
-        private static final boolean kBackLeftEncoderInverted = false;
-
-        private static final Distance kBackLeftXPos = Inches.of(-10);
-        private static final Distance kBackLeftYPos = Inches.of(10);
-
-        // Back Right
-        private static final int kBackRightDriveMotorId = 5;
-        private static final int kBackRightSteerMotorId = 4;
-        private static final int kBackRightEncoderId = 2;
-        private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125);
-        private static final boolean kBackRightSteerMotorInverted = true;
-        private static final boolean kBackRightEncoderInverted = false;
-
-        private static final Distance kBackRightXPos = Inches.of(-10);
-        private static final Distance kBackRightYPos = Inches.of(-10);
-
-        public static final SwerveModuleConstants FrontLeft = ConstantCreator
-                .createModuleConstants(
-                        kFrontLeftSteerMotorId,
-                        kFrontLeftDriveMotorId,
-                        kFrontLeftEncoderId,
-                        kFrontLeftEncoderOffset,
-                        kFrontLeftXPos,
-                        kFrontLeftYPos,
-                        kInvertLeftSide,
-                        kFrontLeftSteerMotorInverted,
-                        kFrontLeftEncoderInverted);
-        public static final SwerveModuleConstants FrontRight = ConstantCreator
-                .createModuleConstants(
-                        kFrontRightSteerMotorId,
-                        kFrontRightDriveMotorId,
-                        kFrontRightEncoderId,
-                        kFrontRightEncoderOffset,
-                        kFrontRightXPos,
-                        kFrontRightYPos,
-                        kInvertRightSide,
-                        kFrontRightSteerMotorInverted,
-                        kFrontRightEncoderInverted);
-        public static final SwerveModuleConstants BackLeft = ConstantCreator
-                .createModuleConstants(
-                        kBackLeftSteerMotorId,
-                        kBackLeftDriveMotorId,
-                        kBackLeftEncoderId,
-                        kBackLeftEncoderOffset,
-                        kBackLeftXPos,
-                        kBackLeftYPos,
-                        kInvertLeftSide,
-                        kBackLeftSteerMotorInverted,
-                        kBackLeftEncoderInverted);
-        public static final SwerveModuleConstants BackRight = ConstantCreator
-                .createModuleConstants(
-                        kBackRightSteerMotorId,
-                        kBackRightDriveMotorId,
-                        kBackRightEncoderId,
-                        kBackRightEncoderOffset,
-                        kBackRightXPos,
-                        kBackRightYPos,
-                        kInvertRightSide,
-                        kBackRightSteerMotorInverted,
-                        kBackRightEncoderInverted);
-
-        /**
-         * Creates a CommandSwerveDrivetrain instance. This should only be called once
-         * in your robot
-         * program,.
-         */
-        // public static CommandSwerveDrivetrain createDrivetrain() {
-        // return new CommandSwerveDrivetrain(
-        // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight);
-        // }
-
-        /**
-         * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected
-         * device types.
-         */
-        public static class TunerSwerveDrivetrain extends SwerveDrivetrain {
-            /**
-             * Constructs a CTRE SwerveDrivetrain using the specified constants.
-             *
-             * 

-             * This constructs the underlying hardware devices, so users should not
-             * construct the
-             * devices themselves. If they need the devices, they can access them through
-             * getters in the
-             * classes.
-             *
-             * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
-             * @param modules             Constants for each specific module
-             */
-            public TunerSwerveDrivetrain(
-                    SwerveDrivetrainConstants drivetrainConstants,
-                    SwerveModuleConstants... modules) {
-                super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules);
-            }
-
-            /**
-             * Constructs a CTRE SwerveDrivetrain using the specified constants.
-             *
-             * 

-             * This constructs the underlying hardware devices, so users should not
-             * construct the
-             * devices themselves. If they need the devices, they can access them through
-             * getters in the
-             * classes.
-             *
-             * @param drivetrainConstants     Drivetrain-wide constants for the swerve drive
-             * @param odometryUpdateFrequency The frequency to run the odometry loop. If
-             *                                unspecified or
-             *                                set to 0 Hz, this is 250 Hz on CAN FD, and 100
-             *                                Hz on CAN 2.0.
-             * @param modules                 Constants for each specific module
-             */
-            public TunerSwerveDrivetrain(
-                    SwerveDrivetrainConstants drivetrainConstants,
-                    double odometryUpdateFrequency,
-                    SwerveModuleConstants... modules) {
-                super(
-                        TalonFX::new,
-                        TalonFX::new,
-                        CANcoder::new,
-                        drivetrainConstants,
-                        odometryUpdateFrequency,
-                        modules);
-            }
-
-            /**
-             * Constructs a CTRE SwerveDrivetrain using the specified constants.
-             *
-             * 

-             * This constructs the underlying hardware devices, so users should not
-             * construct the
-             * devices themselves. If they need the devices, they can access them through
-             * getters in the
-             * classes.
-             *
-             * @param drivetrainConstants       Drivetrain-wide constants for the swerve
-             *                                  drive
-             * @param odometryUpdateFrequency   The frequency to run the odometry loop. If
-             *                                  unspecified or
-             *                                  set to 0 Hz, this is 250 Hz on CAN FD, and
-             *                                  100 Hz on CAN 2.0.
-             * @param odometryStandardDeviation The standard deviation for odometry
-             *                                  calculation in the
-             *                                  form [x, y, theta]áµ€, with units in meters
-             *                                  and radians
-             * @param visionStandardDeviation   The standard deviation for vision
-             *                                  calculation in the form
-             *                                  [x, y, theta]áµ€, with units in meters and
-             *                                  radians
-             * @param modules                   Constants for each specific module
-             */
-            public TunerSwerveDrivetrain(
-                    SwerveDrivetrainConstants drivetrainConstants,
-                    double odometryUpdateFrequency,
-                    Matrix odometryStandardDeviation,
-                    Matrix visionStandardDeviation,
-                    SwerveModuleConstants... modules) {
-                super(
-                        TalonFX::new,
-                        TalonFX::new,
-                        CANcoder::new,
-                        drivetrainConstants,
-                        odometryUpdateFrequency,
-                        odometryStandardDeviation,
-                        visionStandardDeviation,
-                        modules);
-            }
-        }
+    // Front Left
+    private static final int kFrontLeftDriveMotorId = 3;
+    private static final int kFrontLeftSteerMotorId = 2;
+    private static final int kFrontLeftEncoderId = 1;
+    private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375);
+    private static final boolean kFrontLeftSteerMotorInverted = true;
+    private static final boolean kFrontLeftEncoderInverted = false;
+
+    private static final Distance kFrontLeftXPos = Inches.of(10);
+    private static final Distance kFrontLeftYPos = Inches.of(10);
+
+    // Front Right
+    private static final int kFrontRightDriveMotorId = 1;
+    private static final int kFrontRightSteerMotorId = 0;
+    private static final int kFrontRightEncoderId = 0;
+    private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875);
+    private static final boolean kFrontRightSteerMotorInverted = true;
+    private static final boolean kFrontRightEncoderInverted = false;
+
+    private static final Distance kFrontRightXPos = Inches.of(10);
+    private static final Distance kFrontRightYPos = Inches.of(-10);
+
+    // Back Left
+    private static final int kBackLeftDriveMotorId = 7;
+    private static final int kBackLeftSteerMotorId = 6;
+    private static final int kBackLeftEncoderId = 3;
+    private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875);
+    private static final boolean kBackLeftSteerMotorInverted = true;
+    private static final boolean kBackLeftEncoderInverted = false;
+
+    private static final Distance kBackLeftXPos = Inches.of(-10);
+    private static final Distance kBackLeftYPos = Inches.of(10);
+
+    // Back Right
+    private static final int kBackRightDriveMotorId = 5;
+    private static final int kBackRightSteerMotorId = 4;
+    private static final int kBackRightEncoderId = 2;
+    private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125);
+    private static final boolean kBackRightSteerMotorInverted = true;
+    private static final boolean kBackRightEncoderInverted = false;
+
+    private static final Distance kBackRightXPos = Inches.of(-10);
+    private static final Distance kBackRightYPos = Inches.of(-10);
+
+    public static final SwerveModuleConstants<
+            TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+        FrontLeft =
+            ConstantCreator.createModuleConstants(
+                kFrontLeftSteerMotorId,
+                kFrontLeftDriveMotorId,
+                kFrontLeftEncoderId,
+                kFrontLeftEncoderOffset,
+                kFrontLeftXPos,
+                kFrontLeftYPos,
+                kInvertLeftSide,
+                kFrontLeftSteerMotorInverted,
+                kFrontLeftEncoderInverted);
+    public static final SwerveModuleConstants<
+            TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+        FrontRight =
+            ConstantCreator.createModuleConstants(
+                kFrontRightSteerMotorId,
+                kFrontRightDriveMotorId,
+                kFrontRightEncoderId,
+                kFrontRightEncoderOffset,
+                kFrontRightXPos,
+                kFrontRightYPos,
+                kInvertRightSide,
+                kFrontRightSteerMotorInverted,
+                kFrontRightEncoderInverted);
+    public static final SwerveModuleConstants<
+            TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+        BackLeft =
+            ConstantCreator.createModuleConstants(
+                kBackLeftSteerMotorId,
+                kBackLeftDriveMotorId,
+                kBackLeftEncoderId,
+                kBackLeftEncoderOffset,
+                kBackLeftXPos,
+                kBackLeftYPos,
+                kInvertLeftSide,
+                kBackLeftSteerMotorInverted,
+                kBackLeftEncoderInverted);
+    public static final SwerveModuleConstants<
+            TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+        BackRight =
+            ConstantCreator.createModuleConstants(
+                kBackRightSteerMotorId,
+                kBackRightDriveMotorId,
+                kBackRightEncoderId,
+                kBackRightEncoderOffset,
+                kBackRightXPos,
+                kBackRightYPos,
+                kInvertRightSide,
+                kBackRightSteerMotorInverted,
+                kBackRightEncoderInverted);
+
+    /**
+     * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot
+     * program,.
+     */
+    // public static CommandSwerveDrivetrain createDrivetrain() {
+    // return new CommandSwerveDrivetrain(
+    // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight);
+    // }
+
+    /**
+     * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types.
+     */
+    public static class TunerSwerveDrivetrain extends SwerveDrivetrain {
+      /**
+       * Constructs a CTRE SwerveDrivetrain using the specified constants.
+       *
+       * 
This constructs the underlying hardware devices, so users should not construct the
+       * devices themselves. If they need the devices, they can access them through getters in the
+       * classes.
+       *
+       * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
+       * @param modules Constants for each specific module
+       */
+      public TunerSwerveDrivetrain(
+          SwerveDrivetrainConstants drivetrainConstants,
+          SwerveModuleConstants... modules) {
+        super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules);
+      }
+
+      /**
+       * Constructs a CTRE SwerveDrivetrain using the specified constants.
+       *
+       * 
This constructs the underlying hardware devices, so users should not construct the
+       * devices themselves. If they need the devices, they can access them through getters in the
+       * classes.
+       *
+       * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
+       * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or
+       *     set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0.
+       * @param modules Constants for each specific module
+       */
+      public TunerSwerveDrivetrain(
+          SwerveDrivetrainConstants drivetrainConstants,
+          double odometryUpdateFrequency,
+          SwerveModuleConstants... modules) {
+        super(
+            TalonFX::new,
+            TalonFX::new,
+            CANcoder::new,
+            drivetrainConstants,
+            odometryUpdateFrequency,
+            modules);
+      }
+
+      /**
+       * Constructs a CTRE SwerveDrivetrain using the specified constants.
+       *
+       * 
This constructs the underlying hardware devices, so users should not construct the
+       * devices themselves. If they need the devices, they can access them through getters in the
+       * classes.
+       *
+       * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
+       * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or
+       *     set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0.
+       * @param odometryStandardDeviation The standard deviation for odometry calculation in the
+       *     form [x, y, theta]áµ€, with units in meters and radians
+       * @param visionStandardDeviation The standard deviation for vision calculation in the form
+       *     [x, y, theta]áµ€, with units in meters and radians
+       * @param modules Constants for each specific module
+       */
+      public TunerSwerveDrivetrain(
+          SwerveDrivetrainConstants drivetrainConstants,
+          double odometryUpdateFrequency,
+          Matrix odometryStandardDeviation,
+          Matrix visionStandardDeviation,
+          SwerveModuleConstants... modules) {
+        super(
+            TalonFX::new,
+            TalonFX::new,
+            CANcoder::new,
+            drivetrainConstants,
+            odometryUpdateFrequency,
+            odometryStandardDeviation,
+            visionStandardDeviation,
+            modules);
+      }
     }
-
-    public class VisionConstants {
-        // AprilTag layout
-        public static AprilTagFieldLayout aprilTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
-
-        // Camera names, must match names configured on coprocessor
-        public static String camera0Name = "camera_0";
-        public static String camera1Name = "camera_1";
-
-        // Robot to camera transforms
-        // (Not used by Limelight, configure in web UI instead)
-        public static Transform3d robotToCamera0 = new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0));
-        public static Transform3d robotToCamera1 = new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI));
-
-        // Basic filtering thresholds
-        public static double maxAmbiguity = 0.3;
-        public static double maxZError = 0.75;
-
-        // Standard deviation baselines, for 1 meter distance and 1 tag
-        // (Adjusted automatically based on distance and # of tags)
-        public static double linearStdDevBaseline = 0.02; // Meters
-        public static double angularStdDevBaseline = 0.06; // Radians
-
-        // Standard deviation multipliers for each camera
-        // (Adjust to trust some cameras more than others)
-        public static double[] cameraStdDevFactors = new double[] {
-                1.0, // Camera 0
-                1.0 // Camera 1
+  }
+
+  public class VisionConstants {
+    // AprilTag layout
+    public static AprilTagFieldLayout aprilTagLayout =
+        AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
+
+    // Camera names, must match names configured on coprocessor
+    public static String camera0Name = "camera_0";
+    public static String camera1Name = "camera_1";
+
+    // Robot to camera transforms
+    // (Not used by Limelight, configure in web UI instead)
+    public static Transform3d robotToCamera0 =
+        new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0));
+    public static Transform3d robotToCamera1 =
+        new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI));
+
+    // Basic filtering thresholds
+    public static double maxAmbiguity = 0.3;
+    public static double maxZError = 0.75;
+
+    // Standard deviation baselines, for 1 meter distance and 1 tag
+    // (Adjusted automatically based on distance and # of tags)
+    public static double linearStdDevBaseline = 0.02; // Meters
+    public static double angularStdDevBaseline = 0.06; // Radians
+
+    // Standard deviation multipliers for each camera
+    // (Adjust to trust some cameras more than others)
+    public static double[] cameraStdDevFactors =
+        new double[] {
+          1.0, // Camera 0
+          1.0 // Camera 1
         };
 
-        // Multipliers to apply for MegaTag 2 observations
-        public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve
-        public static double angularStdDevMegatag2Factor = Double.POSITIVE_INFINITY; // No rotation data available
-    }
-
-    public static class IntakeConstants {
-        public static final int kPivotMotorID = 8;
-        public static final int kRollerMotorID = 9;
-
-        public static final double kPivotMotorSpeed = 0.5;
-        public static final double kRollerMotorSpeed = 0.5;
-
-        // Change Gear Ratios later
-        public static final double kPivotMotorGearRatio = 1.0;
-        public static final double kRollerMotorGearRatio = 1.0;
-    }
-
-    public static class OperatorConstants {
-        public static final Joystick auxStick = new Joystick(7);
-        public static JoystickButton kIntakeButton1 = new JoystickButton(auxStick, 4);
-        public static JoystickButton kIntakeButton2 = new JoystickButton(auxStick, 5);
-        public static JoystickButton kIntakeButton3 = new JoystickButton(auxStick, 6);
-        public static JoystickButton kIntakeButton4 = new JoystickButton(auxStick, 7);
-    }
+    // Multipliers to apply for MegaTag 2 observations
+    public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve
+    public static double angularStdDevMegatag2Factor =
+        Double.POSITIVE_INFINITY; // No rotation data available
+  }
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index df9520b..d32bb59 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -37,37 +37,34 @@ public class RobotContainer {
   public RobotContainer() {
     switch (Constants.kCurrentMode) {
       case REAL:
-        drive = new Drive(
-            new GyroIOPigeon2(),
-            new ModuleIOTalonFX(ModuleConstants.FrontLeft),
-            new ModuleIOTalonFX(ModuleConstants.FrontRight),
-            new ModuleIOTalonFX(ModuleConstants.BackLeft),
-            new ModuleIOTalonFX(ModuleConstants.BackRight));
+        drive =
+            new Drive(
+                new GyroIOPigeon2(),
+                new ModuleIOTalonFX(ModuleConstants.FrontLeft),
+                new ModuleIOTalonFX(ModuleConstants.FrontRight),
+                new ModuleIOTalonFX(ModuleConstants.BackLeft),
+                new ModuleIOTalonFX(ModuleConstants.BackRight));
         vision = new Vision(null, null);
         break;
       case SIM:
-        drive = new Drive(
-            new GyroIO() {
-            },
-            new ModuleIOSim(ModuleConstants.FrontLeft),
-            new ModuleIOSim(ModuleConstants.FrontRight),
-            new ModuleIOSim(ModuleConstants.BackLeft),
-            new ModuleIOSim(ModuleConstants.BackRight));
+        drive =
+            new Drive(
+                new GyroIO() {},
+                new ModuleIOSim(ModuleConstants.FrontLeft),
+                new ModuleIOSim(ModuleConstants.FrontRight),
+                new ModuleIOSim(ModuleConstants.BackLeft),
+                new ModuleIOSim(ModuleConstants.BackRight));
         vision = new Vision(null, null);
         break;
       case REPLAY:
       default:
-        drive = new Drive(
-            new GyroIO() {
-            },
-            new ModuleIO() {
-            },
-            new ModuleIO() {
-            },
-            new ModuleIO() {
-            },
-            new ModuleIO() {
-            });
+        drive =
+            new Drive(
+                new GyroIO() {},
+                new ModuleIO() {},
+                new ModuleIO() {},
+                new ModuleIO() {},
+                new ModuleIO() {});
         vision = new Vision(null, new CameraIO[] {});
         break;
     }
@@ -89,7 +86,8 @@ private void configureBindings() {
                 () -> -driver.getLeftX(), // ySupplier
                 () -> {
                   Pose2d robotPose = RobotState.getInstance().getEstimatedPose();
-                  Translation2d target = AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d());
+                  Translation2d target =
+                      AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d());
 
                   Translation2d delta = target.minus(robotPose.getTranslation());
 
@@ -104,16 +102,12 @@ private void configureBindings() {
                 drive,
                 () -> RobotState.getInstance().getEstimatedPose(),
                 () -> Hub.innerCenterPoint.toTranslation2d()));
-
-    Constants.OperatorConstants.kIntakeButton1.whileTrue(intake.runPivot());
-    Constants.OperatorConstants.kIntakeButton2.whileTrue(intake.runFeeder());
-    Constants.OperatorConstants.kIntakeButton3.whileTrue(intake.runPivotBack());
-    Constants.OperatorConstants.kIntakeButton4.whileTrue(intake.runFeederBack());
   }
 
   public void robotPeriodic() {
-    OdometryObservation obs = new OdometryObservation(
-        Timer.getTimestamp(), drive.getModulePositions(), drive.getRawGyroRotation());
+    OdometryObservation obs =
+        new OdometryObservation(
+            Timer.getTimestamp(), drive.getModulePositions(), drive.getRawGyroRotation());
     RobotState.getInstance().addOdometryObservation(obs);
   }
 
diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
index d0e1322..002a794 100644
--- a/src/main/java/frc/robot/subsystems/intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -7,14 +7,13 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Constants.IntakeConstants;
 import org.littletonrobotics.junction.Logger;
 
 public class Intake extends SubsystemBase {
   /** Creates a new Intake. */
   private final IntakeIO io;
 
-  private final IntakeIOAutoLogged inputs = new IntakeIOAutoLogged();
+  private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged();
 
   public Intake(IntakeIO io) {
     this.io = io;
@@ -23,8 +22,7 @@ public Intake(IntakeIO io) {
   /**
    * Command to run the pivot
    *
-   * @return runs the pivot at a speed on every iteration until end when it stops
-   *         the running
+   * @return runs the pivot at a speed on every iteration until end when it stops the running
    */
   public Command runPivot() {
     return Commands.runEnd(
@@ -36,8 +34,7 @@ public Command runPivot() {
   /**
    * Command to run the pivot back
    *
-   * @return runs the pivot at a speed on every iteration until end when it stops
-   *         the running
+   * @return runs the pivot at a speed on every iteration until end when it stops the running
    */
   public Command runPivotBack() {
     return Commands.runEnd(
@@ -49,8 +46,7 @@ public Command runPivotBack() {
   /**
    * Command to run the feeder
    *
-   * @return runs the feeder at a speed on every iteration until end when it stops
-   *         the running
+   * @return runs the feeder at a speed on every iteration until end when it stops the running
    */
   public Command runFeeder() {
     return Commands.runEnd(
@@ -62,8 +58,7 @@ public Command runFeeder() {
   /**
    * Command to run the feeder backward
    *
-   * @return runs the feeder at a speed on every iteration until end when it stops
-   *         the running
+   * @return runs the feeder at a speed on every iteration until end when it stops the running
    */
   public Command runFeederBack() {
     return Commands.runEnd(
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
new file mode 100644
index 0000000..93ef3f4
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -0,0 +1,13 @@
+package frc.robot.subsystems.intake;
+
+public final class IntakeConstants {
+  public static final int kPivotMotorID = 8;
+  public static final int kRollerMotorID = 9;
+
+  public static final double kPivotMotorSpeed = 0.5;
+  public static final double kRollerMotorSpeed = 0.5;
+
+  // Change Gear Ratios later
+  public static final double kPivotMotorGearRatio = 1.0;
+  public static final double kRollerMotorGearRatio = 1.0;
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
index c111976..448a479 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
@@ -3,8 +3,7 @@
 import org.littletonrobotics.junction.AutoLog;
 
 /**
- * The {@code IntakeIO} class provides methods for interacting with the intake
- * motors and updating
+ * The {@code IntakeIO} class provides methods for interacting with the intake motors and updating
  * the intake inputs.
  *
  * @author Ryan Hefferon
@@ -13,8 +12,7 @@
  * @author Julien Precourt
  */
 public interface IntakeIO {
-  default void updateInputs(IntakeIOInputs inputs) {
-  }
+  default void updateInputs(IntakeIOInputs inputs) {}
 
   @AutoLog
   public static class IntakeIOInputs {
@@ -33,14 +31,12 @@ public static class IntakeIOInputs {
    *
    * @param speed determines the speed of the pivot on a scale of -1 to 1
    */
-  default void setPivotSpeed(double speed) {
-  }
+  default void setPivotSpeed(double speed) {}
 
   /**
    * method to set the speed of the wheel
    *
    * @param speed determines the speed of the wheel on a scale of -1 to 1
    */
-  default void setWheelSpeed(double speed) {
-  }
+  default void setWheelSpeed(double speed) {}
 }
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java
index 79f728f..b963bf7 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java
@@ -9,7 +9,6 @@
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import edu.wpi.first.math.util.Units;
-import frc.robot.Constants.IntakeConstants;
 
 public class IntakeIOHardware implements IntakeIO {
   private SparkMax pivotMotor = new SparkMax(IntakeConstants.kPivotMotorID, MotorType.kBrushless);
@@ -21,7 +20,8 @@ public class IntakeIOHardware implements IntakeIO {
   public IntakeIOHardware() {
     pivotConfig = new SparkMaxConfig();
     wheelMotor.getConfigurator().apply(wheelMotorConfig);
-    pivotMotor.configure(pivotConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
+    pivotMotor.configure(
+        pivotConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
   }
 
   @Override
@@ -36,8 +36,10 @@ public void setWheelSpeed(double speed) {
 
   @Override
   public void updateInputs(IntakeIOInputs inputs) {
-    inputs.pivotVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(pivotEncoder.getVelocity());
-    inputs.wheelVelocityRadPerSec = Units.rotationsToRadians(wheelMotor.getVelocity().getValueAsDouble());
+    inputs.pivotVelocityRadPerSec =
+        Units.rotationsPerMinuteToRadiansPerSecond(pivotEncoder.getVelocity());
+    inputs.wheelVelocityRadPerSec =
+        Units.rotationsToRadians(wheelMotor.getVelocity().getValueAsDouble());
     inputs.pivotPositionRad = Units.rotationsToRadians(pivotEncoder.getPosition());
     inputs.wheelPositionRad = Units.rotationsToRadians(wheelMotor.getPosition().getValueAsDouble());
     inputs.pivotAppliedVolts = pivotMotor.getAppliedOutput();
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java
index d4f8313..4d514d0 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java
@@ -5,7 +5,6 @@
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
-import frc.robot.Constants.IntakeConstants;
 
 public class IntakeIOSim implements IntakeIO {
 
@@ -21,15 +20,17 @@ public class IntakeIOSim implements IntakeIO {
   private double wheelAppliedVolts = 0.0;
 
   public IntakeIOSim() {
-    pivotSim = new DCMotorSim(
-        LinearSystemId.createDCMotorSystem(
-            pivotGearbox, 0.025, IntakeConstants.kPivotMotorGearRatio),
-        pivotGearbox);
-
-    wheelSim = new DCMotorSim(
-        LinearSystemId.createDCMotorSystem(
-            wheelGearbox, 0.025, IntakeConstants.kRollerMotorGearRatio),
-        wheelGearbox);
+    pivotSim =
+        new DCMotorSim(
+            LinearSystemId.createDCMotorSystem(
+                pivotGearbox, 0.025, IntakeConstants.kPivotMotorGearRatio),
+            pivotGearbox);
+
+    wheelSim =
+        new DCMotorSim(
+            LinearSystemId.createDCMotorSystem(
+                wheelGearbox, 0.025, IntakeConstants.kRollerMotorGearRatio),
+            wheelGearbox);
   }
 
   @Override

From 46e77dae81bef8358ac394a9ad44046076581c38 Mon Sep 17 00:00:00 2001
From: Maxwell Morgan 
Date: Thu, 26 Feb 2026 18:13:33 -0500
Subject: [PATCH 39/61] Reformat files

---
 .vscode/settings.json                         |   2 +-
 src/main/java/frc/robot/Constants.java        |   4 +-
 src/main/java/frc/robot/RobotContainer.java   |   6 +-
 .../java/frc/robot/control/Configurable.java  |   9 +-
 .../frc/robot/control/DefaultControls.java    |  49 +-
 .../frc/robot/control/DriverController.java   | 446 +++++++++---------
 .../frc/robot/control/DriverControls.java     | 176 ++++---
 .../java/frc/robot/control/ZoneControls.java  |  11 +-
 .../shooter/turret/TurretIOSparkMax.java      |   6 +-
 src/main/java/frc/robot/util/Zone.java        | 212 +++++----
 10 files changed, 451 insertions(+), 470 deletions(-)

diff --git a/.vscode/settings.json b/.vscode/settings.json
index 10020d0..85e3c8b 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -70,5 +70,5 @@
   "[java]": {
     "editor.defaultFormatter": "redhat.java"
   },
-  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx8G -Xms100m -Xlog:disable"
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx16G -Xms100m -Xlog:disable"
 }
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 3b85590..5864c2c 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -43,7 +43,5 @@ public static void disableHAL() {
 
   public static RobotConfig kRobotConfig;
 
-  public static final class DeviceIDs {
-    
-  }
+  public static final class DeviceIDs {}
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 482ac4e..976fe7e 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -40,12 +40,10 @@ public class RobotContainer {
   private final CommandXboxController driver =
       new CommandXboxController(Constants.kDriverControllerPort);
 
-  private Drive drive;
-  private Vision vision;
-  private Intake intake;
   private Drive drive;
   private Shooter leftShooter;
   private Shooter rightShooter;
+  private Intake intake;
   // private Vision vision;
 
   public RobotContainer() {
@@ -118,7 +116,7 @@ private void configureBindings() {
     driver.povDown().whileTrue(DriveCommands.crabWalk(drive, Direction.SOUTH));
     driver.povDownLeft().whileTrue(DriveCommands.crabWalk(drive, Direction.SOUTHWEST));
     driver.povLeft().whileTrue(DriveCommands.crabWalk(drive, Direction.WEST));
-    driver.povUpLeft().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTHWEST));    
+    driver.povUpLeft().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTHWEST));
 
     driver.rightBumper().whileTrue(Shooter.shootBothAtHub(leftShooter, rightShooter));
 
diff --git a/src/main/java/frc/robot/control/Configurable.java b/src/main/java/frc/robot/control/Configurable.java
index 65823c8..ee661a0 100644
--- a/src/main/java/frc/robot/control/Configurable.java
+++ b/src/main/java/frc/robot/control/Configurable.java
@@ -1,12 +1,11 @@
 package frc.robot.control;
 
 /**
- * Represents any class that registers a group of bindings or settings
- * during robot initialization.
- * 
+ * Represents any class that registers a group of bindings or settings during robot initialization.
+ *
  * 
Call {@link #configure()} once from RobotContainer during initialization.
  */
 @FunctionalInterface
 public interface Configurable {
-    void configure();
-}
\ No newline at end of file
+  void configure();
+}
diff --git a/src/main/java/frc/robot/control/DefaultControls.java b/src/main/java/frc/robot/control/DefaultControls.java
index 429c07c..470985f 100644
--- a/src/main/java/frc/robot/control/DefaultControls.java
+++ b/src/main/java/frc/robot/control/DefaultControls.java
@@ -7,32 +7,29 @@
 
 public class DefaultControls implements Configurable {
 
-    private final DriverController driver;
-    private final Joystick operator;
-    private final Drive drive;
-    private final Shooter leftShooter;
-    private final Shooter rightShooter;
+  private final DriverController driver;
+  private final Joystick operator;
+  private final Drive drive;
+  private final Shooter leftShooter;
+  private final Shooter rightShooter;
 
-    /** Creates a new DefaultControls. */
-    public DefaultControls(
-            DriverController driver,
-            Joystick operator,
-            Drive drive,
-            Shooter leftShooter,
-            Shooter rightShooter) {
-        this.driver = driver;
-        this.operator = operator;
-        this.drive = drive;
-        this.leftShooter = leftShooter;
-        this.rightShooter = rightShooter;
-    }
+  /** Creates a new DefaultControls. */
+  public DefaultControls(
+      DriverController driver,
+      Joystick operator,
+      Drive drive,
+      Shooter leftShooter,
+      Shooter rightShooter) {
+    this.driver = driver;
+    this.operator = operator;
+    this.drive = drive;
+    this.leftShooter = leftShooter;
+    this.rightShooter = rightShooter;
+  }
 
-    /**
-     * Configure all default commands for the subsystems (e.g. includes joystick driving).
-     */
-    @Override
-    public void configure() {
-        drive.setDefaultCommand(DriveCommands.joystickDrive(drive, null, null, null));
-    }
-    
+  /** Configure all default commands for the subsystems (e.g. includes joystick driving). */
+  @Override
+  public void configure() {
+    drive.setDefaultCommand(DriveCommands.joystickDrive(drive, null, null, null));
+  }
 }
diff --git a/src/main/java/frc/robot/control/DriverController.java b/src/main/java/frc/robot/control/DriverController.java
index e5810c8..74de15a 100644
--- a/src/main/java/frc/robot/control/DriverController.java
+++ b/src/main/java/frc/robot/control/DriverController.java
@@ -5,264 +5,264 @@
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 
 /**
- * Abstracts controller input so DriverControls works with any supported
- * controller type without caring about the underlying hardware.
+ * Abstracts controller input so DriverControls works with any supported controller type without
+ * caring about the underlying hardware.
  */
 public interface DriverController {
 
-    Trigger aCross();
+  Trigger aCross();
 
-    Trigger bCircle();
+  Trigger bCircle();
 
-    Trigger xSquare();
+  Trigger xSquare();
 
-    Trigger yTriangle();
+  Trigger yTriangle();
 
-    Trigger leftBumper();
+  Trigger leftBumper();
 
-    Trigger rightBumper();
+  Trigger rightBumper();
 
-    Trigger leftTrigger();
+  Trigger leftTrigger();
 
-    Trigger rightTrigger();
+  Trigger rightTrigger();
 
-    Trigger dPadUp();
+  Trigger dPadUp();
 
-    Trigger dPadUpLeft();
+  Trigger dPadUpLeft();
 
-    Trigger dPadUpRight();
+  Trigger dPadUpRight();
 
-    Trigger dPadDown();
+  Trigger dPadDown();
 
-    Trigger dPadDownLeft();
+  Trigger dPadDownLeft();
 
-    Trigger dPadDownRight();
+  Trigger dPadDownRight();
 
-    Trigger dPadLeft();
+  Trigger dPadLeft();
 
-    Trigger dPadRight();
+  Trigger dPadRight();
 
-    double getLeftX();
+  double getLeftX();
 
-    double getLeftY();
+  double getLeftY();
 
-    double getRightX();
+  double getRightX();
 
-    double getRightY();
+  double getRightY();
 
-    class XboxDriverController implements DriverController {
-        private final CommandXboxController controller;
+  class XboxDriverController implements DriverController {
+    private final CommandXboxController controller;
 
-        public XboxDriverController(CommandXboxController controller) {
-            this.controller = controller;
-        }
+    public XboxDriverController(CommandXboxController controller) {
+      this.controller = controller;
+    }
+
+    @Override
+    public Trigger aCross() {
+      return controller.a();
+    }
+
+    @Override
+    public Trigger bCircle() {
+      return controller.b();
+    }
+
+    @Override
+    public Trigger xSquare() {
+      return controller.x();
+    }
+
+    @Override
+    public Trigger yTriangle() {
+      return controller.y();
+    }
+
+    @Override
+    public Trigger leftBumper() {
+      return controller.leftBumper();
+    }
+
+    @Override
+    public Trigger rightBumper() {
+      return controller.rightBumper();
+    }
+
+    @Override
+    public Trigger leftTrigger() {
+      return controller.leftTrigger();
+    }
+
+    @Override
+    public Trigger rightTrigger() {
+      return controller.rightTrigger();
+    }
+
+    @Override
+    public Trigger dPadUp() {
+      return controller.povUp();
+    }
+
+    @Override
+    public Trigger dPadUpLeft() {
+      return controller.povUpLeft();
+    }
+
+    @Override
+    public Trigger dPadUpRight() {
+      return controller.povUpRight();
+    }
+
+    @Override
+    public Trigger dPadDown() {
+      return controller.povDown();
+    }
+
+    @Override
+    public Trigger dPadDownLeft() {
+      return controller.povDownLeft();
+    }
+
+    @Override
+    public Trigger dPadDownRight() {
+      return controller.povDownRight();
+    }
+
+    @Override
+    public Trigger dPadLeft() {
+      return controller.povLeft();
+    }
+
+    @Override
+    public Trigger dPadRight() {
+      return controller.povRight();
+    }
+
+    @Override
+    public double getLeftX() {
+      return controller.getLeftX();
+    }
 
-        @Override
-        public Trigger aCross() {
-            return controller.a();
-        }
+    @Override
+    public double getLeftY() {
+      return controller.getLeftY();
+    }
 
-        @Override
-        public Trigger bCircle() {
-            return controller.b();
-        }
+    @Override
+    public double getRightX() {
+      return controller.getRightX();
+    }
 
-        @Override
-        public Trigger xSquare() {
-            return controller.x();
-        }
+    @Override
+    public double getRightY() {
+      return controller.getRightY();
+    }
+  }
 
-        @Override
-        public Trigger yTriangle() {
-            return controller.y();
-        }
+  class PS5DriverController implements DriverController {
+    private final CommandPS5Controller controller;
+
+    public PS5DriverController(CommandPS5Controller controller) {
+      this.controller = controller;
+    }
+
+    @Override
+    public Trigger aCross() {
+      return controller.cross();
+    }
+
+    @Override
+    public Trigger bCircle() {
+      return controller.circle();
+    }
 
-        @Override
-        public Trigger leftBumper() {
-            return controller.leftBumper();
-        }
+    @Override
+    public Trigger xSquare() {
+      return controller.square();
+    }
 
-        @Override
-        public Trigger rightBumper() {
-            return controller.rightBumper();
-        }
+    @Override
+    public Trigger yTriangle() {
+      return controller.triangle();
+    }
 
-        @Override
-        public Trigger leftTrigger() {
-            return controller.leftTrigger();
-        }
+    @Override
+    public Trigger leftBumper() {
+      return controller.L1();
+    }
 
-        @Override
-        public Trigger rightTrigger() {
-            return controller.rightTrigger();
-        }
+    @Override
+    public Trigger rightBumper() {
+      return controller.R1();
+    }
 
-        @Override
-        public Trigger dPadUp() {
-            return controller.povUp();
-        }
+    @Override
+    public Trigger leftTrigger() {
+      return controller.L2();
+    }
 
-        @Override
-        public Trigger dPadUpLeft() {
-            return controller.povUpLeft();
-        }
+    @Override
+    public Trigger rightTrigger() {
+      return controller.R2();
+    }
 
-        @Override
-        public Trigger dPadUpRight() {
-            return controller.povUpRight();
-        }
+    @Override
+    public Trigger dPadUp() {
+      return controller.povUp();
+    }
 
-        @Override
-        public Trigger dPadDown() {
-            return controller.povDown();
-        }
+    @Override
+    public Trigger dPadUpLeft() {
+      return controller.povUpLeft();
+    }
 
-        @Override
-        public Trigger dPadDownLeft() {
-            return controller.povDownLeft();
-        }
+    @Override
+    public Trigger dPadUpRight() {
+      return controller.povUpRight();
+    }
 
-        @Override
-        public Trigger dPadDownRight() {
-            return controller.povDownRight();
-        }
-
-        @Override
-        public Trigger dPadLeft() {
-            return controller.povLeft();
-        }
-
-        @Override
-        public Trigger dPadRight() {
-            return controller.povRight();
-        }
-
-        @Override
-        public double getLeftX() {
-            return controller.getLeftX();
-        }
-
-        @Override
-        public double getLeftY() {
-            return controller.getLeftY();
-        }
-
-        @Override
-        public double getRightX() {
-            return controller.getRightX();
-        }
-
-        @Override
-        public double getRightY() {
-            return controller.getRightY();
-        }
-    }
-
-    class PS5DriverController implements DriverController {
-        private final CommandPS5Controller controller;
-
-        public PS5DriverController(CommandPS5Controller controller) {
-            this.controller = controller;
-        }
-
-        @Override
-        public Trigger aCross() {
-            return controller.cross();
-        }
-
-        @Override
-        public Trigger bCircle() {
-            return controller.circle();
-        }
-
-        @Override
-        public Trigger xSquare() {
-            return controller.square();
-        }
-
-        @Override
-        public Trigger yTriangle() {
-            return controller.triangle();
-        }
-
-        @Override
-        public Trigger leftBumper() {
-            return controller.L1();
-        }
-
-        @Override
-        public Trigger rightBumper() {
-            return controller.R1();
-        }
-
-        @Override
-        public Trigger leftTrigger() {
-            return controller.L2();
-        }
-
-        @Override
-        public Trigger rightTrigger() {
-            return controller.R2();
-        }
-
-        @Override
-        public Trigger dPadUp() {
-            return controller.povUp();
-        }
-
-        @Override
-        public Trigger dPadUpLeft() {
-            return controller.povUpLeft();
-        }
-
-        @Override
-        public Trigger dPadUpRight() {
-            return controller.povUpRight();
-        }
-
-        @Override
-        public Trigger dPadDown() {
-            return controller.povDown();
-        }
-
-        @Override
-        public Trigger dPadDownLeft() {
-            return controller.povDownLeft();
-        }
-
-        @Override
-        public Trigger dPadDownRight() {
-            return controller.povDownRight();
-        }
-
-        @Override
-        public Trigger dPadLeft() {
-            return controller.povLeft();
-        }
-
-        @Override
-        public Trigger dPadRight() {
-            return controller.povRight();
-        }
-
-        @Override
-        public double getLeftX() {
-            return controller.getLeftX();
-        }
-
-        @Override
-        public double getLeftY() {
-            return controller.getLeftY();
-        }
-
-        @Override
-        public double getRightX() {
-            return controller.getRightX();
-        }
-
-        @Override
-        public double getRightY() {
-            return controller.getRightY();
-        }
-    }
-}
\ No newline at end of file
+    @Override
+    public Trigger dPadDown() {
+      return controller.povDown();
+    }
+
+    @Override
+    public Trigger dPadDownLeft() {
+      return controller.povDownLeft();
+    }
+
+    @Override
+    public Trigger dPadDownRight() {
+      return controller.povDownRight();
+    }
+
+    @Override
+    public Trigger dPadLeft() {
+      return controller.povLeft();
+    }
+
+    @Override
+    public Trigger dPadRight() {
+      return controller.povRight();
+    }
+
+    @Override
+    public double getLeftX() {
+      return controller.getLeftX();
+    }
+
+    @Override
+    public double getLeftY() {
+      return controller.getLeftY();
+    }
+
+    @Override
+    public double getRightX() {
+      return controller.getRightX();
+    }
+
+    @Override
+    public double getRightY() {
+      return controller.getRightY();
+    }
+  }
+}
diff --git a/src/main/java/frc/robot/control/DriverControls.java b/src/main/java/frc/robot/control/DriverControls.java
index 16ea199..597782b 100644
--- a/src/main/java/frc/robot/control/DriverControls.java
+++ b/src/main/java/frc/robot/control/DriverControls.java
@@ -1,97 +1,93 @@
 package frc.robot.control;
 
-import org.littletonrobotics.junction.AutoLogOutput;
-
+import edu.wpi.first.wpilibj.Joystick;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.shooter.Shooter;
-
-import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import org.littletonrobotics.junction.AutoLogOutput;
 
 public class DriverControls implements Configurable {
-    @AutoLogOutput(key = "Control/DriverControls/mode")
-    private DriverMode mode = DriverMode.ONE_DRIVER;
-
-    public enum DriverMode {
-        ONE_DRIVER,
-        TWO_DRIVERS;
-    }
-
-    private final DriverController driver;
-    private final Joystick operator;
-    private final Drive drive;
-    private final Shooter leftShooter;
-    private final Shooter rightShooter;
-
-    public DriverControls(
-            DriverController driver,
-            Joystick operator,
-            Drive drive,
-            Shooter leftShooter,
-            Shooter rightShooter) {
-        this.driver = driver;
-        this.operator = operator;
-        this.drive = drive;
-        this.leftShooter = leftShooter;
-        this.rightShooter = rightShooter;
-    }
-
-    @Override
-    public void configure() {
-
-        // Neutral controls (regardless of whether we are in one or two driver mode)
-        driver.xSquare()
-                .onTrue(Shooter.shootBothAtHub(leftShooter, rightShooter));
-
-        configureOneDriver();
-        configureTwoDrivers();
-    }
-
-    /*
-     * Driver Bindings:
-     *
-     * 
LB: Toggle deploy/retract intake LT: Spin intake RB: Shoot LT + A:
-     * backspin intake RT:
-     * Climb RT + A: Unclimb X: reset Gyro D-Pad: CrabWalk LB + RB + Y: Aux Handoff
-     *
-     */
-
-    private void configureOneDriver() {
-        driver.rightBumper().and(this::isOneDriver)
-                .onTrue(Shooter.shootBothAtHub(leftShooter, rightShooter));
-    }
-
-    /*
-     * 
Back up Operator Controls:
-     *
-     * 
Pancake up + down: Pitch of turrets Pancake left + right: rotation of
-     * turrets trigger
-     * button: Fires fuel from turrets
-     *
-     * 
button 7: deploy intake button 8: run intake button 9: retract intake
-     *
-     * 
button 6: climber up button 4: climber down
-     *
-     * 
thumb button: Driver Handoff
-     */
-    private void configureTwoDrivers() {
-        
-    }
-
-    private boolean isOneDriver() {
-        return mode == DriverMode.ONE_DRIVER;
-    }
-
-    private boolean isTwoDrivers() {
-        return mode == DriverMode.TWO_DRIVERS;
-    }
-
-    public void setMode(DriverMode mode) {
-        this.mode = mode;
-        configure();
-    }
-
-    public DriverMode getMode() {
-        return mode;
-    }
+  @AutoLogOutput(key = "Control/DriverControls/mode")
+  private DriverMode mode = DriverMode.ONE_DRIVER;
+
+  public enum DriverMode {
+    ONE_DRIVER,
+    TWO_DRIVERS;
+  }
+
+  private final DriverController driver;
+  private final Joystick operator;
+  private final Drive drive;
+  private final Shooter leftShooter;
+  private final Shooter rightShooter;
+
+  public DriverControls(
+      DriverController driver,
+      Joystick operator,
+      Drive drive,
+      Shooter leftShooter,
+      Shooter rightShooter) {
+    this.driver = driver;
+    this.operator = operator;
+    this.drive = drive;
+    this.leftShooter = leftShooter;
+    this.rightShooter = rightShooter;
+  }
+
+  @Override
+  public void configure() {
+
+    // Neutral controls (regardless of whether we are in one or two driver mode)
+    driver.xSquare().onTrue(Shooter.shootBothAtHub(leftShooter, rightShooter));
+
+    configureOneDriver();
+    configureTwoDrivers();
+  }
+
+  /*
+   * Driver Bindings:
+   *
+   * 
LB: Toggle deploy/retract intake LT: Spin intake RB: Shoot LT + A:
+   * backspin intake RT:
+   * Climb RT + A: Unclimb X: reset Gyro D-Pad: CrabWalk LB + RB + Y: Aux Handoff
+   *
+   */
+
+  private void configureOneDriver() {
+    driver
+        .rightBumper()
+        .and(this::isOneDriver)
+        .onTrue(Shooter.shootBothAtHub(leftShooter, rightShooter));
+  }
+
+  /*
+   * 
Back up Operator Controls:
+   *
+   * 
Pancake up + down: Pitch of turrets Pancake left + right: rotation of
+   * turrets trigger
+   * button: Fires fuel from turrets
+   *
+   * 
button 7: deploy intake button 8: run intake button 9: retract intake
+   *
+   * 
button 6: climber up button 4: climber down
+   *
+   * 
thumb button: Driver Handoff
+   */
+  private void configureTwoDrivers() {}
+
+  private boolean isOneDriver() {
+    return mode == DriverMode.ONE_DRIVER;
+  }
+
+  private boolean isTwoDrivers() {
+    return mode == DriverMode.TWO_DRIVERS;
+  }
+
+  public void setMode(DriverMode mode) {
+    this.mode = mode;
+    configure();
+  }
+
+  public DriverMode getMode() {
+    return mode;
+  }
 }
diff --git a/src/main/java/frc/robot/control/ZoneControls.java b/src/main/java/frc/robot/control/ZoneControls.java
index fdbc875..d5d6081 100644
--- a/src/main/java/frc/robot/control/ZoneControls.java
+++ b/src/main/java/frc/robot/control/ZoneControls.java
@@ -2,10 +2,9 @@
 
 public class ZoneControls implements Configurable {
 
-    @Override
-    public void configure() {
-        // TODO Auto-generated method stub
-        throw new UnsupportedOperationException("Unimplemented method 'configure'");
-    }
-    
+  @Override
+  public void configure() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'configure'");
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
index 29cb3f0..7d1f133 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
@@ -19,7 +19,6 @@
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.math.geometry.Rotation2d;
 import frc.robot.subsystems.shooter.ShooterConstants.TurretConstants;
-
 import java.util.function.DoubleSupplier;
 
 public class TurretIOSparkMax implements TurretIO {
@@ -45,10 +44,7 @@ public TurretIOSparkMax(int motorID) {
             2 * Math.PI / TurretConstants.kGearRatio) // No absolute encoder...
         .velocityConversionFactor(2 * Math.PI / TurretConstants.kGearRatio / 60.0);
 
-    config
-        .closedLoop
-        .positionWrappingEnabled(false)
-        .feedbackSensor(FeedbackSensor.kPrimaryEncoder);
+    config.closedLoop.positionWrappingEnabled(false).feedbackSensor(FeedbackSensor.kPrimaryEncoder);
 
     config
         .softLimit
diff --git a/src/main/java/frc/robot/util/Zone.java b/src/main/java/frc/robot/util/Zone.java
index cc56fd4..6231b5a 100644
--- a/src/main/java/frc/robot/util/Zone.java
+++ b/src/main/java/frc/robot/util/Zone.java
@@ -2,7 +2,6 @@
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-
 import java.util.List;
 import java.util.function.Supplier;
 
@@ -21,125 +20,124 @@
  */
 public interface Zone {
 
-    /**
-     * Returns a Trigger that is active when the supplied translation is inside this zone.
-     *
-     * @param translation a Supplier providing the current Translation2d to check
-     * @return a Trigger that polls containment
-     */
-    Trigger contains(Supplier translation);
-
-    /** Returns a zone that is the union (A ∪ B) of this zone and another. */
-    default Zone union(Zone other) {
-        return translation -> this.contains(translation).or(other.contains(translation));
+  /**
+   * Returns a Trigger that is active when the supplied translation is inside this zone.
+   *
+   * @param translation a Supplier providing the current Translation2d to check
+   * @return a Trigger that polls containment
+   */
+  Trigger contains(Supplier translation);
+
+  /** Returns a zone that is the union (A ∪ B) of this zone and another. */
+  default Zone union(Zone other) {
+    return translation -> this.contains(translation).or(other.contains(translation));
+  }
+
+  /** Returns a zone that is the intersection (A ∩ B) of this zone and another. */
+  default Zone intersection(Zone other) {
+    return translation -> this.contains(translation).and(other.contains(translation));
+  }
+
+  /**
+   * Returns a zone representing the difference (A \ B): points in this zone that are NOT in the
+   * other zone.
+   */
+  default Zone difference(Zone other) {
+    return translation -> this.contains(translation).and(other.contains(translation).negate());
+  }
+
+  /** Returns the complement of this zone (points NOT in this zone). */
+  default Zone complement() {
+    return translation -> this.contains(translation).negate();
+  }
+
+  /**
+   * A circular zone defined by a center point and a radius. A translation is inside if its distance
+   * to the center is less than the radius.
+   */
+  class CircleZone implements Zone {
+    private final Translation2d center;
+    private final double radius;
+
+    public CircleZone(Translation2d center, double radius) {
+      this.center = center;
+      this.radius = radius;
     }
 
-    /** Returns a zone that is the intersection (A ∩ B) of this zone and another. */
-    default Zone intersection(Zone other) {
-        return translation -> this.contains(translation).and(other.contains(translation));
+    @Override
+    public Trigger contains(Supplier translation) {
+      return new Trigger(() -> translation.get().getDistance(center) < radius);
     }
-
-    /**
-     * Returns a zone representing the difference (A \ B): points in this zone
-     * that are NOT in the other zone.
-     */
-    default Zone difference(Zone other) {
-        return translation -> this.contains(translation).and(other.contains(translation).negate());
+  }
+
+  /**
+   * An axis-aligned rectangular zone defined by two corner points. A translation is inside if its x
+   * and y coordinates fall within the bounding box.
+   */
+  class RectangleZone implements Zone {
+    private final double minX, maxX, minY, maxY;
+
+    public RectangleZone(Translation2d cornerA, Translation2d cornerB) {
+      this.minX = Math.min(cornerA.getX(), cornerB.getX());
+      this.maxX = Math.max(cornerA.getX(), cornerB.getX());
+      this.minY = Math.min(cornerA.getY(), cornerB.getY());
+      this.maxY = Math.max(cornerA.getY(), cornerB.getY());
     }
 
-    /** Returns the complement of this zone (points NOT in this zone). */
-    default Zone complement() {
-        return translation -> this.contains(translation).negate();
+    @Override
+    public Trigger contains(Supplier translation) {
+      return new Trigger(
+          () -> {
+            Translation2d t = translation.get();
+            return t.getX() >= minX && t.getX() <= maxX && t.getY() >= minY && t.getY() <= maxY;
+          });
     }
+  }
+
+  /**
+   * A polygonal zone defined by an ordered list of vertices.
+   *
+   * 
Uses a cross-product (winding) approach: for each edge of the polygon, the point must be on
+   * the same side (left side for CCW winding). Works correctly for convex polygons. For concave
+   * polygons, use a ray-casting approach instead.
+   */
+  class PolygonZone implements Zone {
+    private final List vertices;
 
     /**
-     * A circular zone defined by a center point and a radius.
-     * A translation is inside if its distance to the center is less than the radius.
+     * @param vertices ordered vertices of the polygon (CCW winding for correct results)
      */
-    class CircleZone implements Zone {
-        private final Translation2d center;
-        private final double radius;
-
-        public CircleZone(Translation2d center, double radius) {
-            this.center = center;
-            this.radius = radius;
-        }
-
-        @Override
-        public Trigger contains(Supplier translation) {
-            return new Trigger(() -> translation.get().getDistance(center) < radius);
-        }
+    public PolygonZone(List vertices) {
+      if (vertices.size() < 3) {
+        throw new IllegalArgumentException("A polygon must have at least 3 vertices.");
+      }
+      this.vertices = List.copyOf(vertices);
     }
 
-    /**
-     * An axis-aligned rectangular zone defined by two corner points.
-     * A translation is inside if its x and y coordinates fall within the bounding box.
-     */
-    class RectangleZone implements Zone {
-        private final double minX, maxX, minY, maxY;
-
-        public RectangleZone(Translation2d cornerA, Translation2d cornerB) {
-            this.minX = Math.min(cornerA.getX(), cornerB.getX());
-            this.maxX = Math.max(cornerA.getX(), cornerB.getX());
-            this.minY = Math.min(cornerA.getY(), cornerB.getY());
-            this.maxY = Math.max(cornerA.getY(), cornerB.getY());
-        }
-
-        @Override
-        public Trigger contains(Supplier translation) {
-            return new Trigger(() -> {
-                Translation2d t = translation.get();
-                return t.getX() >= minX && t.getX() <= maxX
-                    && t.getY() >= minY && t.getY() <= maxY;
-            });
-        }
+    @Override
+    public Trigger contains(Supplier translation) {
+      return new Trigger(() -> isInsidePolygon(translation.get()));
     }
 
     /**
-     * A polygonal zone defined by an ordered list of vertices.
-     *
-     * Uses a cross-product (winding) approach: for each edge of the polygon,
-     * the point must be on the same side (left side for CCW winding).
-     * Works correctly for convex polygons. For concave polygons, use a
-     * ray-casting approach instead.
+     * Ray-casting algorithm for point-in-polygon detection. Works for both convex and concave
+     * (simple) polygons.
      */
-    class PolygonZone implements Zone {
-        private final List vertices;
-
-        /**
-         * @param vertices ordered vertices of the polygon (CCW winding for correct results)
-         */
-        public PolygonZone(List vertices) {
-            if (vertices.size() < 3) {
-                throw new IllegalArgumentException("A polygon must have at least 3 vertices.");
-            }
-            this.vertices = List.copyOf(vertices);
-        }
-
-        @Override
-        public Trigger contains(Supplier translation) {
-            return new Trigger(() -> isInsidePolygon(translation.get()));
-        }
-
-        /**
-         * Ray-casting algorithm for point-in-polygon detection.
-         * Works for both convex and concave (simple) polygons.
-         */
-        private boolean isInsidePolygon(Translation2d point) {
-            int n = vertices.size();
-            boolean inside = false;
-            double px = point.getX();
-            double py = point.getY();
-
-            for (int i = 0, j = n - 1; i < n; j = i++) {
-                double xi = vertices.get(i).getX(), yi = vertices.get(i).getY();
-                double xj = vertices.get(j).getX(), yj = vertices.get(j).getY();
-
-                boolean intersects = ((yi > py) != (yj > py))
-                        && (px < (xj - xi) * (py - yi) / (yj - yi) + xi);
-                if (intersects) inside = !inside;
-            }
-            return inside;
-        }
+    private boolean isInsidePolygon(Translation2d point) {
+      int n = vertices.size();
+      boolean inside = false;
+      double px = point.getX();
+      double py = point.getY();
+
+      for (int i = 0, j = n - 1; i < n; j = i++) {
+        double xi = vertices.get(i).getX(), yi = vertices.get(i).getY();
+        double xj = vertices.get(j).getX(), yj = vertices.get(j).getY();
+
+        boolean intersects =
+            ((yi > py) != (yj > py)) && (px < (xj - xi) * (py - yi) / (yj - yi) + xi);
+        if (intersects) inside = !inside;
+      }
+      return inside;
     }
-}
\ No newline at end of file
+  }
+}

From f00b044040a92edce50b056da0f94a398c48c105 Mon Sep 17 00:00:00 2001
From: Maxwell Morgan 
Date: Thu, 26 Feb 2026 18:32:52 -0500
Subject: [PATCH 40/61] Fix guts errors

---
 src/main/java/frc/robot/Constants.java        | 847 +-----------------
 src/main/java/frc/robot/Robot.java            |   2 +-
 src/main/java/frc/robot/RobotContainer.java   |  75 +-
 .../java/frc/robot/subsystems/guts/Guts.java  |  36 +-
 .../robot/subsystems/guts/GutsConstants.java  |   8 +
 .../frc/robot/subsystems/guts/GutsIO.java     |  12 +-
 .../frc/robot/subsystems/guts/GutsIOSim.java  |  36 +-
 .../robot/subsystems/guts/GutsIOSparkMax.java |  64 +-
 8 files changed, 133 insertions(+), 947 deletions(-)
 create mode 100644 src/main/java/frc/robot/subsystems/guts/GutsConstants.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index be40673..9715bb2 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -9,7 +9,6 @@
 
 import com.pathplanner.lib.config.RobotConfig;
 import edu.wpi.first.wpilibj.RobotBase;
-import java.util.Map;
 
 /**
  * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running
@@ -42,835 +41,39 @@ public static void disableHAL() {
     kDisableHAL = true;
   }
 
-  public static final class DriveConstants {
+  public static RobotConfig kRobotConfig;
 
-    public static final class ModuleConfigs {
+  public static final class DeviceIDs {
+    public static final int kPigeon = 0;
 
-      public static record ModuleConfig(
-          int driveMotorID, int angleMotorID, int canCoderID, Rotation2d angleOffset) {}
+    public static final int kLeftFrontModuleDrive = 0;
+    public static final int kLeftFrontModuleAzimuth = 0;
+    public static final int kLeftFrontModuleEncoder = 0;
 
-      /** Module 0 (front left) configs. */
-      public static final ModuleConfig FrontLeft =
-          new ModuleConfig(1, 2, 19, Rotation2d.fromDegrees(304.36523 - 180));
+    public static final int kRightFrontModuleDrive = 0;
+    public static final int kRightFrontModuleAzimuth = 0;
+    public static final int kRightFrontModuleEncoder = 0;
 
-      /** Module 1 (front right) configs. */
-      public static final ModuleConfig FrontRight =
-          new ModuleConfig(2, 4, 20, Rotation2d.fromDegrees(206.455));
+    public static final int kLeftBackModuleDrive = 0;
+    public static final int kLeftBackModuleAzimuth = 0;
+    public static final int kLeftBackModuleEncoder = 0;
 
-      /** Module 2 (back left) configs. */
-      public static final ModuleConfig BackLeft =
-          new ModuleConfig(5, 6, 21, Rotation2d.fromDegrees(35.419922 + 180));
+    public static final int kRightBackModuleDrive = 0;
+    public static final int kRightBackModuleAzimuth = 0;
+    public static final int kRightBackModuleEncoder = 0;
 
-      /** Module 3 (back right) configs. */
-      public static final ModuleConfig BackRight =
-          new ModuleConfig(7, 8, 22, Rotation2d.fromDegrees(116.89453));
-    }
+    public static final int kLeftTurretFlywheel = 0;
+    public static final int kLeftTurretHood = 0;
+    public static final int kLeftTurretAzimuth = 0;
 
-    // TunerConstants doesn't include these constants
-    public static final double kOdometryFrequency =
-        ModuleConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0;
-    public static final double kDriveBaseRadius =
-        Math.max(
-            Math.max(
-                Math.hypot(
-                    ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY),
-                Math.hypot(
-                    ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY)),
-            Math.max(
-                Math.hypot(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY),
-                Math.hypot(
-                    ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY)));
+    public static final int kRightTurretFlywheel = 0;
+    public static final int kRightTurretHood = 0;
+    public static final int kRightTurretAzimuth = 0;
 
-    public static final Translation2d[] kModuleTranslations =
-        new Translation2d[] {
-          new Translation2d(
-              ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY),
-          new Translation2d(
-              ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY),
-          new Translation2d(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY),
-          new Translation2d(
-              ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY)
-        };
+    public static final int kLeftGuts = 0;
+    public static final int kRightGuts = 0;
 
-    // PathPlanner config constants
-    public static final double kRobotMassKG = 74.088;
-    public static final double kRobotMOI = 6.883;
-    /** Coefficient of friction */
-    public static final double kWheelCOF = 1.2;
-
-    public static final RobotConfig kPathplannerConfig =
-        new RobotConfig(
-            kRobotMOI,
-            kRobotMOI,
-            new ModuleConfig(
-                ModuleConstants.FrontLeft.WheelRadius,
-                ModuleConstants.kSpeedAt12Volts.in(MetersPerSecond),
-                kWheelCOF,
-                DCMotor.getKrakenX60Foc(1)
-                    .withReduction(ModuleConstants.FrontLeft.DriveMotorGearRatio),
-                ModuleConstants.FrontLeft.SlipCurrent,
-                1),
-            kModuleTranslations);
-
-    public static final IdleMode kDriveIdleMode = IdleMode.kBrake;
-    public static final IdleMode kAngleIdleMode = IdleMode.kBrake;
-    public static final double kDrivePower = 1;
-    public static final double kAnglePower = .9;
-
-    public static final boolean kInvertGyro = false; // Always ensure Gyro is CCW+ CW-
-
-    // drivetrain constants
-    public static final double kTrackWidth = Units.inchesToMeters(24.75);
-    public static final double kWheelBase = Units.inchesToMeters(24.75);
-    public static final double kWheelDiameter = Units.inchesToMeters(4.0);
-    public static final double kWheelRadius = kWheelDiameter / 2.0;
-    public static final double kWheelCircumference = kWheelDiameter * Math.PI;
-
-    // Swerve kinematics, don't change
-    public static final SwerveDriveKinematics swerveKinematics =
-        new SwerveDriveKinematics(
-            new Translation2d(kWheelBase / 2.0, kTrackWidth / 2.0), // front left
-            new Translation2d(kWheelBase / 2.0, -kTrackWidth / 2.0), // front right
-            new Translation2d(-kWheelBase / 2.0, kTrackWidth / 2.0), // back left
-            new Translation2d(-kWheelBase / 2.0, -kTrackWidth / 2.0)); // back right
-
-    // gear ratios
-    public static final double kDriveGearRatio = (6.12 / 1.0);
-    public static final double kAngleGearRatio = ((150.0 / 7.0) / 1.0);
-
-    // encoder stuff
-    // meters per rotation
-    public static final double kDriveRevToMeters = kWheelCircumference / (kDriveGearRatio);
-    public static final double kDriveRpmToMetersPerSecond = kDriveRevToMeters / 60;
-
-    /** The number of degrees that a single rotation of the turn motor turns the // wheel. */
-    public static final double kDegreesPerTurnRotation = 360 / kAngleGearRatio;
-
-    // motor inverts, check these
-    public static final boolean kAngleMotorInvert = true;
-    public static final InvertedValue kDriveMotorInvert = InvertedValue.CounterClockwise_Positive;
-
-    /* Angle Encoder Invert */
-    public static final boolean kCanCoderInvert = false;
-
-    /* Swerve Current Limiting */
-    public static final int kAngleContinuousCurrentLimit = 20;
-    public static final int kAnglePeakCurrentLimit = 40;
-    public static final double kAnglePeakCurrentDuration = 0.1;
-    public static final boolean kAngleEnableCurrentLimit = true;
-
-    public static final int kDriveSupplyCurrentLimit = 60;
-    public static final boolean kDriveSupplyCurrentLimitEnable = true;
-    public static final int kDriveSupplyCurrentThreshold = 60;
-    public static final double kDriveSupplyTimeThreshold = 0.1;
-
-    public static final boolean kDriveEnableCurrentLimit = true;
-
-    /*
-     * These values are used by the drive falcon to ramp in open loop and closed
-     * loop driving.
-     * We found a small open loop ramp (0.25) helps with tread wear, tipping, etc
-     */
-    public static final double kOpenLoopRamp = 0.25;
-    public static final double kClosedLoopRamp = 0.0;
-
-    /* Angle Motor PID Values */
-    public static final double kAngleKP = 0.015;
-    public static final double kAngleKI = 0;
-    public static final double kAngleKD = 0;
-    public static final double kAngleKF = 0;
-
-    /* Drive Motor PID Values */
-
-    public static final double kDriveKP = 0.01;
-    public static final double kDriveKI = 0.0;
-    public static final double kDriveKD = 0.0;
-
-    public static final double kDriveKS = (0.32 / 12);
-    public static final double kDriveKV = (1.988 / 12);
-    public static final double kDriveKA = (1.0449 / 12);
-
-    /* Swerve Profiling Values */
-    /** Meters per second. */
-    public static final double kPhysicalMaxSpeed = 5.0;
-
-    public static final double kMaxTeleDriveSpeed = 4.5;
-    /** Radians per second. */
-    public static final double kPhysicalMaxAngularSpeed = 2 * 2 * Math.PI;
-    /** Radians per second. */
-    public static final double kMaxTeleAngularSpeed = kPhysicalMaxAngularSpeed / 2;
-
-    public static final double kMaxAngularAccelerationSpeed = 4 / Math.PI;
-    /** Radians per second. */
-    public static final double kMaxTeleAngularAccelerationSpeed = kMaxAngularAccelerationSpeed / 2;
-
-    public static final double kDeadband = 0.08;
-
-    public static final Map kDistances =
-        Map.of(
-            0, 0.0,
-            1, 1.0,
-            2, 2.0,
-            3, 3.0,
-            4, 4.0);
-
-    public static class ModuleConstants {
-      // Both sets of gains need to be tuned to your individual robot.
-
-      // The steer motor uses any SwerveModule.SteerRequestType control request with
-      // the
-      // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
-      private static final Slot0Configs steerGains =
-          new Slot0Configs()
-              .withKP(100)
-              .withKI(0)
-              .withKD(0.5)
-              .withKS(0.1)
-              .withKV(1.91)
-              .withKA(0)
-              .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
-      // When using closed-loop control, the drive motor uses the control
-      // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
-      private static final Slot0Configs driveGains =
-          new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124);
-
-      // The closed-loop output type to use for the steer motors;
-      // This affects the PID/FF gains for the steer motors
-      private static final ClosedLoopOutputType kSteerClosedLoopOutput =
-          ClosedLoopOutputType.Voltage;
-      // The closed-loop output type to use for the drive motors;
-      // This affects the PID/FF gains for the drive motors
-      private static final ClosedLoopOutputType kDriveClosedLoopOutput =
-          ClosedLoopOutputType.Voltage;
-
-      // The type of motor used for the drive motor
-      private static final DriveMotorArrangement kDriveMotorType =
-          DriveMotorArrangement.TalonFX_Integrated;
-      // The type of motor used for the drive motor
-      private static final SteerMotorArrangement kSteerMotorType =
-          SteerMotorArrangement.TalonFX_Integrated;
-
-      // The remote sensor feedback type to use for the steer motors;
-      // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to
-      // RemoteCANcoder
-      private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
-
-      // The stator current at which the wheels start to slip;
-      // This needs to be tuned to your individual robot
-      private static final Current kSlipCurrent = Amps.of(120.0);
-
-      // Initial configs for the drive and steer motors and the azimuth encoder; these
-      // cannot be null.
-      // Some configs will be overwritten; check the `with*InitialConfigs()` API
-      // documentation.
-      private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
-      private static final TalonFXConfiguration steerInitialConfigs =
-          new TalonFXConfiguration()
-              .withCurrentLimits(
-                  new CurrentLimitsConfigs()
-                      // Swerve azimuth does not require much torque output, so we can set a
-                      // relatively
-                      // low
-                      // stator current limit to help avoid brownouts without impacting performance.
-                      .withStatorCurrentLimit(Amps.of(60))
-                      .withStatorCurrentLimitEnable(true));
-      private static final CANcoderConfiguration encoderInitialConfigs =
-          new CANcoderConfiguration();
-      // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
-      private static final Pigeon2Configuration pigeonConfigs = null;
-
-      // CAN bus that the devices are located on;
-      // All swerve devices must share the same CAN bus
-      public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot");
-
-      // Theoretical free speed (m/s) at 12 V applied output;
-      // This needs to be tuned to your individual robot
-      public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69);
-
-      // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
-      // This may need to be tuned to your individual robot
-      private static final double kCoupleRatio = 3.8181818181818183;
-
-      private static final double kDriveGearRatio = 7.363636363636365;
-      private static final double kSteerGearRatio = 15.42857142857143;
-      private static final Distance kWheelRadius = Inches.of(2.167);
-
-      private static final boolean kInvertLeftSide = false;
-      private static final boolean kInvertRightSide = true;
-
-      private static final int kPigeonId = 1;
-
-      // These are only used for simulation
-      private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004);
-      private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025);
-      // Simulated voltage necessary to overcome friction
-      private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
-      private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
-
-      public static final SwerveDrivetrainConstants DrivetrainConstants =
-          new SwerveDrivetrainConstants()
-              .withCANBusName(kCANBus.getName())
-              .withPigeon2Id(kPigeonId)
-              .withPigeon2Configs(pigeonConfigs);
-
-      private static final SwerveModuleConstantsFactory<
-              TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
-          ConstantCreator =
-              new SwerveModuleConstantsFactory<
-                      TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>()
-                  .withDriveMotorGearRatio(kDriveGearRatio)
-                  .withSteerMotorGearRatio(kSteerGearRatio)
-                  .withCouplingGearRatio(kCoupleRatio)
-                  .withWheelRadius(kWheelRadius)
-                  .withSteerMotorGains(steerGains)
-                  .withDriveMotorGains(driveGains)
-                  .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput)
-                  .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput)
-                  .withSlipCurrent(kSlipCurrent)
-                  .withSpeedAt12Volts(kSpeedAt12Volts)
-                  .withDriveMotorType(kDriveMotorType)
-                  .withSteerMotorType(kSteerMotorType)
-                  .withFeedbackSource(kSteerFeedbackType)
-                  .withDriveMotorInitialConfigs(driveInitialConfigs)
-                  .withSteerMotorInitialConfigs(steerInitialConfigs)
-                  .withEncoderInitialConfigs(encoderInitialConfigs)
-                  .withSteerInertia(kSteerInertia)
-                  .withDriveInertia(kDriveInertia)
-                  .withSteerFrictionVoltage(kSteerFrictionVoltage)
-                  .withDriveFrictionVoltage(kDriveFrictionVoltage);
-
-      // Front Left
-      private static final int kFrontLeftDriveMotorId = 3;
-      private static final int kFrontLeftSteerMotorId = 2;
-      private static final int kFrontLeftEncoderId = 1;
-      private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375);
-      private static final boolean kFrontLeftSteerMotorInverted = true;
-      private static final boolean kFrontLeftEncoderInverted = false;
-
-      private static final Distance kFrontLeftXPos = Inches.of(10);
-      private static final Distance kFrontLeftYPos = Inches.of(10);
-
-      // Front Right
-      private static final int kFrontRightDriveMotorId = 1;
-      private static final int kFrontRightSteerMotorId = 0;
-      private static final int kFrontRightEncoderId = 0;
-      private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875);
-      private static final boolean kFrontRightSteerMotorInverted = true;
-      private static final boolean kFrontRightEncoderInverted = false;
-
-      private static final Distance kFrontRightXPos = Inches.of(10);
-      private static final Distance kFrontRightYPos = Inches.of(-10);
-
-      // Back Left
-      private static final int kBackLeftDriveMotorId = 7;
-      private static final int kBackLeftSteerMotorId = 6;
-      private static final int kBackLeftEncoderId = 3;
-      private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875);
-      private static final boolean kBackLeftSteerMotorInverted = true;
-      private static final boolean kBackLeftEncoderInverted = false;
-
-      private static final Distance kBackLeftXPos = Inches.of(-10);
-      private static final Distance kBackLeftYPos = Inches.of(10);
-
-      // Back Right
-      private static final int kBackRightDriveMotorId = 5;
-      private static final int kBackRightSteerMotorId = 4;
-      private static final int kBackRightEncoderId = 2;
-      private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125);
-      private static final boolean kBackRightSteerMotorInverted = true;
-      private static final boolean kBackRightEncoderInverted = false;
-
-      private static final Distance kBackRightXPos = Inches.of(-10);
-      private static final Distance kBackRightYPos = Inches.of(-10);
-
-      public static final SwerveModuleConstants<
-              TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
-          FrontLeft =
-              ConstantCreator.createModuleConstants(
-                  kFrontLeftSteerMotorId,
-                  kFrontLeftDriveMotorId,
-                  kFrontLeftEncoderId,
-                  kFrontLeftEncoderOffset,
-                  kFrontLeftXPos,
-                  kFrontLeftYPos,
-                  kInvertLeftSide,
-                  kFrontLeftSteerMotorInverted,
-                  kFrontLeftEncoderInverted);
-      public static final SwerveModuleConstants<
-              TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
-          FrontRight =
-              ConstantCreator.createModuleConstants(
-                  kFrontRightSteerMotorId,
-                  kFrontRightDriveMotorId,
-                  kFrontRightEncoderId,
-                  kFrontRightEncoderOffset,
-                  kFrontRightXPos,
-                  kFrontRightYPos,
-                  kInvertRightSide,
-                  kFrontRightSteerMotorInverted,
-                  kFrontRightEncoderInverted);
-      public static final SwerveModuleConstants<
-              TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
-          BackLeft =
-              ConstantCreator.createModuleConstants(
-                  kBackLeftSteerMotorId,
-                  kBackLeftDriveMotorId,
-                  kBackLeftEncoderId,
-                  kBackLeftEncoderOffset,
-                  kBackLeftXPos,
-                  kBackLeftYPos,
-                  kInvertLeftSide,
-                  kBackLeftSteerMotorInverted,
-                  kBackLeftEncoderInverted);
-      public static final SwerveModuleConstants<
-              TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
-          BackRight =
-              ConstantCreator.createModuleConstants(
-                  kBackRightSteerMotorId,
-                  kBackRightDriveMotorId,
-                  kBackRightEncoderId,
-                  kBackRightEncoderOffset,
-                  kBackRightXPos,
-                  kBackRightYPos,
-                  kInvertRightSide,
-                  kBackRightSteerMotorInverted,
-                  kBackRightEncoderInverted);
-
-      /**
-       * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot
-       * program,.
-       */
-      // public static CommandSwerveDrivetrain createDrivetrain() {
-      // return new CommandSwerveDrivetrain(
-      // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight);
-      // }
-
-      /**
-       * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types.
-       */
-      public static class TunerSwerveDrivetrain
-          extends SwerveDrivetrain {
-        /**
-         * Constructs a CTRE SwerveDrivetrain using the specified constants.
-         *
-         * 
This constructs the underlying hardware devices, so users should not construct the
-         * devices themselves. If they need the devices, they can access them through getters in the
-         * classes.
-         *
-         * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
-         * @param modules Constants for each specific module
-         */
-        public TunerSwerveDrivetrain(
-            SwerveDrivetrainConstants drivetrainConstants,
-            SwerveModuleConstants... modules) {
-          super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules);
-        }
-
-        /**
-         * Constructs a CTRE SwerveDrivetrain using the specified constants.
-         *
-         * 
This constructs the underlying hardware devices, so users should not construct the
-         * devices themselves. If they need the devices, they can access them through getters in the
-         * classes.
-         *
-         * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
-         * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or
-         *     set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0.
-         * @param modules Constants for each specific module
-         */
-        public TunerSwerveDrivetrain(
-            SwerveDrivetrainConstants drivetrainConstants,
-            double odometryUpdateFrequency,
-            SwerveModuleConstants... modules) {
-          super(
-              TalonFX::new,
-              TalonFX::new,
-              CANcoder::new,
-              drivetrainConstants,
-              odometryUpdateFrequency,
-              modules);
-        }
-
-        /**
-         * Constructs a CTRE SwerveDrivetrain using the specified constants.
-         *
-         * 
This constructs the underlying hardware devices, so users should not construct the
-         * devices themselves. If they need the devices, they can access them through getters in the
-         * classes.
-         *
-         * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
-         * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or
-         *     set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0.
-         * @param odometryStandardDeviation The standard deviation for odometry calculation in the
-         *     form [x, y, theta]áµ€, with units in meters and radians
-         * @param visionStandardDeviation The standard deviation for vision calculation in the form
-         *     [x, y, theta]áµ€, with units in meters and radians
-         * @param modules Constants for each specific module
-         */
-        public TunerSwerveDrivetrain(
-            SwerveDrivetrainConstants drivetrainConstants,
-            double odometryUpdateFrequency,
-            Matrix odometryStandardDeviation,
-            Matrix visionStandardDeviation,
-            SwerveModuleConstants... modules) {
-          super(
-              TalonFX::new,
-              TalonFX::new,
-              CANcoder::new,
-              drivetrainConstants,
-              odometryUpdateFrequency,
-              odometryStandardDeviation,
-              visionStandardDeviation,
-              modules);
-        }
-      }
-    }
-  }
-
-  public class ModuleConstants {
-    // Both sets of gains need to be tuned to your individual robot.
-
-    // The steer motor uses any SwerveModule.SteerRequestType control request with
-    // the
-    // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
-    private static final Slot0Configs steerGains =
-        new Slot0Configs()
-            .withKP(100)
-            .withKI(0)
-            .withKD(0.5)
-            .withKS(0.1)
-            .withKV(1.91)
-            .withKA(0)
-            .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
-    // When using closed-loop control, the drive motor uses the control
-    // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
-    private static final Slot0Configs driveGains =
-        new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124);
-
-    // The closed-loop output type to use for the steer motors;
-    // This affects the PID/FF gains for the steer motors
-    private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage;
-    // The closed-loop output type to use for the drive motors;
-    // This affects the PID/FF gains for the drive motors
-    private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage;
-
-    // The type of motor used for the drive motor
-    private static final DriveMotorArrangement kDriveMotorType =
-        DriveMotorArrangement.TalonFX_Integrated;
-    // The type of motor used for the drive motor
-    private static final SteerMotorArrangement kSteerMotorType =
-        SteerMotorArrangement.TalonFX_Integrated;
-
-    // The remote sensor feedback type to use for the steer motors;
-    // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to
-    // RemoteCANcoder
-    private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
-
-    // The stator current at which the wheels start to slip;
-    // This needs to be tuned to your individual robot
-    private static final Current kSlipCurrent = Amps.of(120.0);
-
-    // Initial configs for the drive and steer motors and the azimuth encoder; these
-    // cannot be null.
-    // Some configs will be overwritten; check the `with*InitialConfigs()` API
-    // documentation.
-    private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
-    private static final TalonFXConfiguration steerInitialConfigs =
-        new TalonFXConfiguration()
-            .withCurrentLimits(
-                new CurrentLimitsConfigs()
-                    // Swerve azimuth does not require much torque output, so we can set a
-                    // relatively
-                    // low
-                    // stator current limit to help avoid brownouts without impacting performance.
-                    .withStatorCurrentLimit(Amps.of(60))
-                    .withStatorCurrentLimitEnable(true));
-    private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
-    // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
-    private static final Pigeon2Configuration pigeonConfigs = null;
-
-    // CAN bus that the devices are located on;
-    // All swerve devices must share the same CAN bus
-    public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot");
-
-    // Theoretical free speed (m/s) at 12 V applied output;
-    // This needs to be tuned to your individual robot
-    public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69);
-
-    // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
-    // This may need to be tuned to your individual robot
-    private static final double kCoupleRatio = 3.8181818181818183;
-
-    private static final double kDriveGearRatio = 7.363636363636365;
-    private static final double kSteerGearRatio = 15.42857142857143;
-    private static final Distance kWheelRadius = Inches.of(2.167);
-
-    private static final boolean kInvertLeftSide = false;
-    private static final boolean kInvertRightSide = true;
-
-    private static final int kPigeonId = 1;
-
-    // These are only used for simulation
-    private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004);
-    private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025);
-    // Simulated voltage necessary to overcome friction
-    private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
-    private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
-
-    public static final SwerveDrivetrainConstants DrivetrainConstants =
-        new SwerveDrivetrainConstants()
-            .withCANBusName(kCANBus.getName())
-            .withPigeon2Id(kPigeonId)
-            .withPigeon2Configs(pigeonConfigs);
-
-    private static final SwerveModuleConstantsFactory<
-            TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
-        ConstantCreator =
-            new SwerveModuleConstantsFactory<
-                    TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>()
-                .withDriveMotorGearRatio(kDriveGearRatio)
-                .withSteerMotorGearRatio(kSteerGearRatio)
-                .withCouplingGearRatio(kCoupleRatio)
-                .withWheelRadius(kWheelRadius)
-                .withSteerMotorGains(steerGains)
-                .withDriveMotorGains(driveGains)
-                .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput)
-                .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput)
-                .withSlipCurrent(kSlipCurrent)
-                .withSpeedAt12Volts(kSpeedAt12Volts)
-                .withDriveMotorType(kDriveMotorType)
-                .withSteerMotorType(kSteerMotorType)
-                .withFeedbackSource(kSteerFeedbackType)
-                .withDriveMotorInitialConfigs(driveInitialConfigs)
-                .withSteerMotorInitialConfigs(steerInitialConfigs)
-                .withEncoderInitialConfigs(encoderInitialConfigs)
-                .withSteerInertia(kSteerInertia)
-                .withDriveInertia(kDriveInertia)
-                .withSteerFrictionVoltage(kSteerFrictionVoltage)
-                .withDriveFrictionVoltage(kDriveFrictionVoltage);
-
-    // Front Left
-    private static final int kFrontLeftDriveMotorId = 3;
-    private static final int kFrontLeftSteerMotorId = 2;
-    private static final int kFrontLeftEncoderId = 1;
-    private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375);
-    private static final boolean kFrontLeftSteerMotorInverted = true;
-    private static final boolean kFrontLeftEncoderInverted = false;
-
-    private static final Distance kFrontLeftXPos = Inches.of(10);
-    private static final Distance kFrontLeftYPos = Inches.of(10);
-
-    // Front Right
-    private static final int kFrontRightDriveMotorId = 1;
-    private static final int kFrontRightSteerMotorId = 0;
-    private static final int kFrontRightEncoderId = 0;
-    private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875);
-    private static final boolean kFrontRightSteerMotorInverted = true;
-    private static final boolean kFrontRightEncoderInverted = false;
-
-    private static final Distance kFrontRightXPos = Inches.of(10);
-    private static final Distance kFrontRightYPos = Inches.of(-10);
-
-    // Back Left
-    private static final int kBackLeftDriveMotorId = 7;
-    private static final int kBackLeftSteerMotorId = 6;
-    private static final int kBackLeftEncoderId = 3;
-    private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875);
-    private static final boolean kBackLeftSteerMotorInverted = true;
-    private static final boolean kBackLeftEncoderInverted = false;
-
-    private static final Distance kBackLeftXPos = Inches.of(-10);
-    private static final Distance kBackLeftYPos = Inches.of(10);
-
-    // Back Right
-    private static final int kBackRightDriveMotorId = 5;
-    private static final int kBackRightSteerMotorId = 4;
-    private static final int kBackRightEncoderId = 2;
-    private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125);
-    private static final boolean kBackRightSteerMotorInverted = true;
-    private static final boolean kBackRightEncoderInverted = false;
-
-    private static final Distance kBackRightXPos = Inches.of(-10);
-    private static final Distance kBackRightYPos = Inches.of(-10);
-
-    public static final SwerveModuleConstants<
-            TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
-        FrontLeft =
-            ConstantCreator.createModuleConstants(
-                kFrontLeftSteerMotorId,
-                kFrontLeftDriveMotorId,
-                kFrontLeftEncoderId,
-                kFrontLeftEncoderOffset,
-                kFrontLeftXPos,
-                kFrontLeftYPos,
-                kInvertLeftSide,
-                kFrontLeftSteerMotorInverted,
-                kFrontLeftEncoderInverted);
-    public static final SwerveModuleConstants<
-            TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
-        FrontRight =
-            ConstantCreator.createModuleConstants(
-                kFrontRightSteerMotorId,
-                kFrontRightDriveMotorId,
-                kFrontRightEncoderId,
-                kFrontRightEncoderOffset,
-                kFrontRightXPos,
-                kFrontRightYPos,
-                kInvertRightSide,
-                kFrontRightSteerMotorInverted,
-                kFrontRightEncoderInverted);
-    public static final SwerveModuleConstants<
-            TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
-        BackLeft =
-            ConstantCreator.createModuleConstants(
-                kBackLeftSteerMotorId,
-                kBackLeftDriveMotorId,
-                kBackLeftEncoderId,
-                kBackLeftEncoderOffset,
-                kBackLeftXPos,
-                kBackLeftYPos,
-                kInvertLeftSide,
-                kBackLeftSteerMotorInverted,
-                kBackLeftEncoderInverted);
-    public static final SwerveModuleConstants<
-            TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
-        BackRight =
-            ConstantCreator.createModuleConstants(
-                kBackRightSteerMotorId,
-                kBackRightDriveMotorId,
-                kBackRightEncoderId,
-                kBackRightEncoderOffset,
-                kBackRightXPos,
-                kBackRightYPos,
-                kInvertRightSide,
-                kBackRightSteerMotorInverted,
-                kBackRightEncoderInverted);
-
-    /**
-     * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot
-     * program,.
-     */
-    // public static CommandSwerveDrivetrain createDrivetrain() {
-    // return new CommandSwerveDrivetrain(
-    // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight);
-    // }
-
-    /**
-     * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types.
-     */
-    public static class TunerSwerveDrivetrain extends SwerveDrivetrain {
-      /**
-       * Constructs a CTRE SwerveDrivetrain using the specified constants.
-       *
-       * 
This constructs the underlying hardware devices, so users should not construct the
-       * devices themselves. If they need the devices, they can access them through getters in the
-       * classes.
-       *
-       * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
-       * @param modules Constants for each specific module
-       */
-      public TunerSwerveDrivetrain(
-          SwerveDrivetrainConstants drivetrainConstants,
-          SwerveModuleConstants... modules) {
-        super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules);
-      }
-
-      /**
-       * Constructs a CTRE SwerveDrivetrain using the specified constants.
-       *
-       * 
This constructs the underlying hardware devices, so users should not construct the
-       * devices themselves. If they need the devices, they can access them through getters in the
-       * classes.
-       *
-       * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
-       * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or
-       *     set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0.
-       * @param modules Constants for each specific module
-       */
-      public TunerSwerveDrivetrain(
-          SwerveDrivetrainConstants drivetrainConstants,
-          double odometryUpdateFrequency,
-          SwerveModuleConstants... modules) {
-        super(
-            TalonFX::new,
-            TalonFX::new,
-            CANcoder::new,
-            drivetrainConstants,
-            odometryUpdateFrequency,
-            modules);
-      }
-
-      /**
-       * Constructs a CTRE SwerveDrivetrain using the specified constants.
-       *
-       * 
This constructs the underlying hardware devices, so users should not construct the
-       * devices themselves. If they need the devices, they can access them through getters in the
-       * classes.
-       *
-       * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
-       * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or
-       *     set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0.
-       * @param odometryStandardDeviation The standard deviation for odometry calculation in the
-       *     form [x, y, theta]áµ€, with units in meters and radians
-       * @param visionStandardDeviation The standard deviation for vision calculation in the form
-       *     [x, y, theta]áµ€, with units in meters and radians
-       * @param modules Constants for each specific module
-       */
-      public TunerSwerveDrivetrain(
-          SwerveDrivetrainConstants drivetrainConstants,
-          double odometryUpdateFrequency,
-          Matrix odometryStandardDeviation,
-          Matrix visionStandardDeviation,
-          SwerveModuleConstants... modules) {
-        super(
-            TalonFX::new,
-            TalonFX::new,
-            CANcoder::new,
-            drivetrainConstants,
-            odometryUpdateFrequency,
-            odometryStandardDeviation,
-            visionStandardDeviation,
-            modules);
-      }
-    }
-  }
-
-  public class VisionConstants {
-    // AprilTag layout
-    public static AprilTagFieldLayout aprilTagLayout =
-        AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
-
-    // Camera names, must match names configured on coprocessor
-    public static String camera0Name = "camera_0";
-    public static String camera1Name = "camera_1";
-
-    // Robot to camera transforms
-    // (Not used by Limelight, configure in web UI instead)
-    public static Transform3d robotToCamera0 =
-        new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0));
-    public static Transform3d robotToCamera1 =
-        new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI));
-
-    // Basic filtering thresholds
-    public static double maxAmbiguity = 0.3;
-    public static double maxZError = 0.75;
-
-    // Standard deviation baselines, for 1 meter distance and 1 tag
-    // (Adjusted automatically based on distance and # of tags)
-    public static double linearStdDevBaseline = 0.02; // Meters
-    public static double angularStdDevBaseline = 0.06; // Radians
-
-    // Standard deviation multipliers for each camera
-    // (Adjust to trust some cameras more than others)
-    public static double[] cameraStdDevFactors =
-        new double[] {
-          1.0, // Camera 0
-          1.0 // Camera 1
-        };
-
-    // Multipliers to apply for MegaTag 2 observations
-    public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve
-    public static double angularStdDevMegatag2Factor =
-        Double.POSITIVE_INFINITY; // No rotation data available
+    public static final int kIntakeDrive = 0;
+    public static final int kIntakePivot = 0;
   }
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 7e2b4d0..1fb2f38 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -75,7 +75,7 @@ public Robot() {
   @Override
   public void robotPeriodic() {
     CachedSupplier.invalidateAll();
-    robotContainer.robotContainerPeriodic();
+    robotContainer.robotPeriodic();
     CommandScheduler.getInstance().run();
 
     RobotVisualizer.getInstance().log("Mechanism3d/Robot");
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 4a7f100..2e9a63e 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -25,6 +25,9 @@
 import frc.robot.subsystems.drive.ModuleIO;
 import frc.robot.subsystems.drive.ModuleIOSim;
 import frc.robot.subsystems.drive.ModuleIOTalonFX;
+import frc.robot.subsystems.guts.Guts;
+import frc.robot.subsystems.guts.Guts.GutSide;
+import frc.robot.subsystems.guts.GutsIOSim;
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.Shooter.ShooterSide;
@@ -35,9 +38,6 @@
 import frc.robot.util.Direction;
 import frc.robot.util.FieldConstants;
 import frc.robot.util.FieldConstants.Hub;
-import frc.robot.subsystems.guts.Guts;
-import frc.robot.subsystems.guts.GutsIO;
-import frc.robot.subsystems.guts.Guts.GutSide;
 
 public class RobotContainer {
   private final CommandXboxController driver =
@@ -54,12 +54,13 @@ public class RobotContainer {
   public RobotContainer() {
     switch (Constants.kCurrentMode) {
       case REAL:
-        drive = new Drive(
-            new GyroIOPigeon2(),
-            new ModuleIOTalonFX(ModuleConstants.FrontLeft),
-            new ModuleIOTalonFX(ModuleConstants.FrontRight),
-            new ModuleIOTalonFX(ModuleConstants.BackLeft),
-            new ModuleIOTalonFX(ModuleConstants.BackRight));
+        drive =
+            new Drive(
+                new GyroIOPigeon2(),
+                new ModuleIOTalonFX(ModuleConstants.FrontLeft),
+                new ModuleIOTalonFX(ModuleConstants.FrontRight),
+                new ModuleIOTalonFX(ModuleConstants.BackLeft),
+                new ModuleIOTalonFX(ModuleConstants.BackRight));
         // vision = new Vision(null, null);
         break;
       case SIM:
@@ -75,46 +76,38 @@ public RobotContainer() {
         rightShooter =
             new Shooter(ShooterSide.RIGHT, new TurretIOSim(), new HoodIOSim(), new FlywheelIOSim());
         // vision = new Vision(null, null);
-        drive = new Drive(
-            new GyroIO() {
-            },
-            new ModuleIOSim(ModuleConstants.FrontLeft),
-            new ModuleIOSim(ModuleConstants.FrontRight),
-            new ModuleIOSim(ModuleConstants.BackLeft),
-            new ModuleIOSim(ModuleConstants.BackRight));
+        drive =
+            new Drive(
+                new GyroIO() {},
+                new ModuleIOSim(ModuleConstants.FrontLeft),
+                new ModuleIOSim(ModuleConstants.FrontRight),
+                new ModuleIOSim(ModuleConstants.BackLeft),
+                new ModuleIOSim(ModuleConstants.BackRight));
         // vision = new Vision(null, null);
-        leftGuts = new Guts(GutSide.LEFT, new GutsIO() {
-
-        });
-        rightGuts = new Guts(GutSide.RIGHT, new GutsIO() {
-
-        });
+        leftGuts = new Guts(GutSide.LEFT, new GutsIOSim());
+        rightGuts = new Guts(GutSide.RIGHT, new GutsIOSim());
 
         break;
       case REPLAY:
       default:
-        drive = new Drive(
-            new GyroIO() {
-            },
-            new ModuleIO() {
-            },
-            new ModuleIO() {
-            },
-            new ModuleIO() {
-            },
-            new ModuleIO() {
-            });
+        drive =
+            new Drive(
+                new GyroIO() {},
+                new ModuleIO() {},
+                new ModuleIO() {},
+                new ModuleIO() {},
+                new ModuleIO() {});
         // vision = new Vision(null, new CameraIO[] {});
         break;
     }
 
     // if (Constants.kCurrentMode == Constants.Mode.REAL) {
-    //   try {
-    //     Constants.kRobotConfig = RobotConfig.fromGUISettings();
-    //   } catch (Exception e) {
-    //     // Handle exception as needed
-    //     e.printStackTrace();
-    //   }
+    // try {
+    // Constants.kRobotConfig = RobotConfig.fromGUISettings();
+    // } catch (Exception e) {
+    // // Handle exception as needed
+    // e.printStackTrace();
+    // }
     // }
 
     // configurePathPlanner();
@@ -152,7 +145,8 @@ private void configureBindings() {
                 () -> -driver.getLeftX(), // ySupplier
                 () -> {
                   Pose2d robotPose = RobotState.getInstance().getEstimatedPose();
-                  Translation2d target = AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d());
+                  Translation2d target =
+                      AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d());
 
                   Translation2d delta = target.minus(robotPose.getTranslation());
 
@@ -166,7 +160,6 @@ private void configureBindings() {
                 drive,
                 () -> RobotState.getInstance().getEstimatedPose(),
                 () -> Hub.innerCenterPoint.toTranslation2d()));
-
   }
 
   public void robotPeriodic() {
diff --git a/src/main/java/frc/robot/subsystems/guts/Guts.java b/src/main/java/frc/robot/subsystems/guts/Guts.java
index e1991b0..9672738 100644
--- a/src/main/java/frc/robot/subsystems/guts/Guts.java
+++ b/src/main/java/frc/robot/subsystems/guts/Guts.java
@@ -4,50 +4,40 @@
 
 package frc.robot.subsystems.guts;
 
-import org.littletonrobotics.junction.Logger;
-
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Constants.GutsConstants;
+import org.littletonrobotics.junction.Logger;
 
 /**
- * This class updates and stores the values of the inputs periodically, and
- * contains commands to run the gut motor forward and backward.
- * 
+ * This class updates and stores the values of the inputs periodically, and contains commands to run
+ * the gut motor forward and backward.
+ *
  * @author Ryan Hefferon
  */
 public class Guts extends SubsystemBase {
 
   private final GutSide side;
-  public final GutsIO io;
-  public GutsIOInputsAutoLogged inputs = new GutsIOInputsAutoLogged();
-  public double speed = (side == GutSide.LEFT) ? (GutsConstants.kGutMotorSpeed) : -(GutsConstants.kGutMotorSpeed);
+  private final GutsIO io;
+  private GutsIOInputsAutoLogged inputs = new GutsIOInputsAutoLogged();
+  private final double speed;
 
   /** Creates a new Guts. */
   public Guts(GutSide side, GutsIO io) {
     this.io = io;
     this.side = side;
+    speed =
+        (side == GutSide.LEFT) ? (GutsConstants.kGutMotorSpeed) : -(GutsConstants.kGutMotorSpeed);
   }
 
-  /**
-   * Runs the gut motor forward at 0.5 speed, then stops it when finished.
-   */
+  /** Runs the gut motor forward at 0.5 speed, then stops it when finished. */
   public Command runGutForward() {
-    return Commands.runEnd(
-      () -> io.setGutMotorSpeed(speed),
-      () -> io.setGutMotorSpeed(0),
-      this);
+    return Commands.runEnd(() -> io.setGutMotorSpeed(speed), () -> io.setGutMotorSpeed(0), this);
   }
 
-  /**
-   * Runs the gut motor backward at 0.5 speed, then stops it when finished.
-   */
+  /** Runs the gut motor backward at 0.5 speed, then stops it when finished. */
   public Command runGutBackward() {
-      return Commands.runEnd(
-        () -> io.setGutMotorSpeed(-speed),
-        () -> io.setGutMotorSpeed(0),
-        this);
+    return Commands.runEnd(() -> io.setGutMotorSpeed(-speed), () -> io.setGutMotorSpeed(0), this);
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/guts/GutsConstants.java b/src/main/java/frc/robot/subsystems/guts/GutsConstants.java
new file mode 100644
index 0000000..37b7bd3
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/guts/GutsConstants.java
@@ -0,0 +1,8 @@
+package frc.robot.subsystems.guts;
+
+public final class GutsConstants {
+
+  public static final double kGutMotorSpeed = 0.5;
+  // Change Gear Ratio later
+  public static final double kGutMotorGearRatio = 0.0;
+}
diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIO.java b/src/main/java/frc/robot/subsystems/guts/GutsIO.java
index 6406750..d6301ca 100644
--- a/src/main/java/frc/robot/subsystems/guts/GutsIO.java
+++ b/src/main/java/frc/robot/subsystems/guts/GutsIO.java
@@ -3,15 +3,15 @@
 import org.littletonrobotics.junction.AutoLog;
 
 /**
- * This IO interface contains the class which initializes all the inputs as well
- * as default methods to update the values of the inputs and set the speed of the motor.
+ * This IO interface contains the class which initializes all the inputs as well as default methods
+ * to update the values of the inputs and set the speed of the motor.
+ *
  * @author Ryan Hefferon
  */
 public interface GutsIO {
 
   /** Updates the values of all the inputs using the physical encoders. */
-  default void updateInputs(GutsIOInputs inputs) {
-  }
+  default void updateInputs(GutsIOInputs inputs) {}
 
   /** Contains all the inputs regarding motors to be stored as data. */
   @AutoLog
@@ -23,7 +23,5 @@ public static class GutsIOInputs {
   }
 
   /** Sets the gut motor to a specific speed ranging from -1.0 to 1.0 */
-  default void setGutMotorSpeed(double speed) {
-  }
-
+  default void setGutMotorSpeed(double speed) {}
 }
diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIOSim.java b/src/main/java/frc/robot/subsystems/guts/GutsIOSim.java
index c3f75df..65cd935 100644
--- a/src/main/java/frc/robot/subsystems/guts/GutsIOSim.java
+++ b/src/main/java/frc/robot/subsystems/guts/GutsIOSim.java
@@ -1,32 +1,27 @@
 package frc.robot.subsystems.guts;
 
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.system.plant.LinearSystemId;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
-import frc.robot.Constants;
-import frc.robot.Constants.GutsConstants;
 
 public class GutsIOSim implements GutsIO {
-private final DCMotor gearbox = DCMotor.getNEO(1);
-private final DCMotorSim sim;
+  private final DCMotor gearbox = DCMotor.getNEO(1);
+  private final DCMotorSim sim;
 
-//private final PIDController pid = new PIDController(1, 0, 0, Constants.kLoopPeriodSeconds);
+  // private final PIDController pid = new PIDController(1, 0, 0, Constants.kLoopPeriodSeconds);
 
-private double appliedVolts = 0.0;
+  private double appliedVolts = 0.0;
 
-public GutsIOSim() {
-    sim = 
+  public GutsIOSim() {
+    sim =
         new DCMotorSim(
-            LinearSystemId.createDCMotorSystem(gearbox, 0.025, GutsConstants.kGutMotorGearRatio), 
-            gearbox
-        );
-}
+            LinearSystemId.createDCMotorSystem(gearbox, 0.025, GutsConstants.kGutMotorGearRatio),
+            gearbox);
+  }
 
-@Override
-public void updateInputs(GutsIOInputs inputs) {
+  @Override
+  public void updateInputs(GutsIOInputs inputs) {
 
     appliedVolts = MathUtil.clamp(appliedVolts, -12.0, 12.0);
 
@@ -35,11 +30,10 @@ public void updateInputs(GutsIOInputs inputs) {
 
     inputs.positionRad = sim.getAngularPositionRotations();
     inputs.velocityRadPerSec = sim.getAngularVelocityRPM();
-}
+  }
 
-@Override
-public void setGutMotorSpeed(double speed) {
+  @Override
+  public void setGutMotorSpeed(double speed) {
     appliedVolts = 12 * speed;
+  }
 }
-
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java b/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java
index e29fc2c..d450b8c 100644
--- a/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java
@@ -1,46 +1,46 @@
 package frc.robot.subsystems.guts;
 
+import com.revrobotics.PersistMode;
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.spark.SparkMax;
-import com.revrobotics.ResetMode;
-import com.revrobotics.PersistMode;
 import com.revrobotics.spark.config.SparkMaxConfig;
-
 import edu.wpi.first.math.util.Units;
-import frc.robot.Constants.GutsConstants;
 
 /**
- * This class contains all of the physical objects: one motor and its
- * corresponding encoder. It also implements the default methods specified in
- * the IO interface to set the speed of the physical motor and update the input
- * values using the encoders.
- * 
+ * This class contains all of the physical objects: one motor and its corresponding encoder. It also
+ * implements the default methods specified in the IO interface to set the speed of the physical
+ * motor and update the input values using the encoders.
+ *
  * @author Ryan Hefferon
  */
 public class GutsIOSparkMax implements GutsIO {
 
-    private final SparkMax gutMotor = new SparkMax(GutsConstants.kGutMotorID, MotorType.kBrushless);
-    private final RelativeEncoder gutEncoder = gutMotor.getEncoder();
-    private final SparkMaxConfig gutMotorConfig;
-
-    public GutsIOSparkMax() {
-        gutMotorConfig = new SparkMaxConfig();
-
-        gutMotor.configure(gutMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
-    }
-
-    @Override
-    public void setGutMotorSpeed(double speed) {
-        gutMotor.set(speed);
-    }
-
-    @Override
-    public void updateInputs(GutsIOInputs inputs) {
-        inputs.velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(gutEncoder.getVelocity());
-        inputs.positionRad = Units.rotationsToRadians(gutEncoder.getPosition());
-        inputs.appliedVolts = gutMotor.getAppliedOutput();
-        inputs.currentDrawAmps = gutMotor.getOutputCurrent();
-    }
-
+  private final SparkMax gutMotor;
+  private final RelativeEncoder gutEncoder;
+  private final SparkMaxConfig gutMotorConfig;
+  private final int motorID;
+
+  public GutsIOSparkMax(int motorID) {
+    this.motorID = motorID;
+    gutMotor = new SparkMax(motorID, MotorType.kBrushless);
+    gutEncoder = gutMotor.getEncoder();
+    gutMotorConfig = new SparkMaxConfig();
+    gutMotor.configure(
+        gutMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
+  }
+
+  @Override
+  public void setGutMotorSpeed(double speed) {
+    gutMotor.set(speed);
+  }
+
+  @Override
+  public void updateInputs(GutsIOInputs inputs) {
+    inputs.velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(gutEncoder.getVelocity());
+    inputs.positionRad = Units.rotationsToRadians(gutEncoder.getPosition());
+    inputs.appliedVolts = gutMotor.getAppliedOutput();
+    inputs.currentDrawAmps = gutMotor.getOutputCurrent();
+  }
 }

From 492f54ea313fd295aa2f35a80e5fd81ee62b3a60 Mon Sep 17 00:00:00 2001
From: Maxwell Morgan 
Date: Mon, 2 Mar 2026 15:31:13 -0500
Subject: [PATCH 41/61] Get this show on the road

---
 .vscode/settings.json                         |   2 +-
 src/main/java/frc/robot/Constants.java        |  78 +++---
 src/main/java/frc/robot/RobotContainer.java   | 118 ++++-----
 src/main/java/frc/robot/RobotState.java       |  10 +-
 .../frc/robot/commands/DriveCommands.java     |  20 +-
 .../frc/robot/control/DefaultControls.java    |   4 +-
 .../frc/robot/control/DriverController.java   |  21 +-
 .../frc/robot/control/DriverControls.java     |  53 +++-
 .../frc/robot/subsystems/drive/Drive.java     |  63 +++--
 .../subsystems/drive/DriveConstants.java      | 139 +++++------
 .../frc/robot/subsystems/drive/GyroIO.java    |   2 +
 .../robot/subsystems/drive/GyroIONavX.java    |   5 +
 .../robot/subsystems/drive/GyroIOPigeon2.java |  13 +-
 .../frc/robot/subsystems/drive/Module.java    |   5 +
 .../robot/subsystems/drive/ModuleIOSim.java   |   2 +-
 .../subsystems/drive/ModuleIOTalonFX.java     |   8 +-
 .../subsystems/drive/ModuleIOTalonFXS.java    |   8 +-
 .../drive/PhoenixOdometryThread.java          |   4 +-
 .../robot/subsystems/guts/GutsConstants.java  |   2 +-
 .../robot/subsystems/guts/GutsIOSparkMax.java |   2 -
 .../frc/robot/subsystems/intake/Intake.java   |  10 +-
 .../subsystems/intake/IntakeConstants.java    |   3 -
 .../subsystems/intake/IntakeIOHardware.java   |  17 +-
 .../subsystems/shooter/turret/Turret.java     |   2 +
 src/main/java/frc/robot/util/Direction.java   |  25 +-
 .../frc/robot/util/tuner-swerve-project.json  | 232 ++++++++++++++++++
 vendordeps/AdvantageKit.json                  |   6 +-
 ...enix6-26.1.0.json => Phoenix6-26.1.1.json} |  62 ++---
 vendordeps/REVLib.json                        |  18 +-
 vendordeps/photonlib.json                     |  12 +-
 30 files changed, 636 insertions(+), 310 deletions(-)
 create mode 100644 src/main/java/frc/robot/util/tuner-swerve-project.json
 rename vendordeps/{Phoenix6-26.1.0.json => Phoenix6-26.1.1.json} (92%)

diff --git a/.vscode/settings.json b/.vscode/settings.json
index 85e3c8b..d139599 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -70,5 +70,5 @@
   "[java]": {
     "editor.defaultFormatter": "redhat.java"
   },
-  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx16G -Xms100m -Xlog:disable"
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx32G -Xms100m -Xlog:disable"
 }
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 9715bb2..ffee2c5 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -9,6 +9,7 @@
 
 import com.pathplanner.lib.config.RobotConfig;
 import edu.wpi.first.wpilibj.RobotBase;
+import frc.robot.subsystems.drive.DriveConstants;
 
 /**
  * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running
@@ -33,7 +34,7 @@ public static enum Mode {
   }
 
   public static final int kDriverControllerPort = 0;
-  public static final int kAuxControllerPort = 1;
+  public static final int kOperatorControllerPort = 1;
 
   public static boolean kDisableHAL = false;
 
@@ -44,36 +45,49 @@ public static void disableHAL() {
   public static RobotConfig kRobotConfig;
 
   public static final class DeviceIDs {
-    public static final int kPigeon = 0;
-
-    public static final int kLeftFrontModuleDrive = 0;
-    public static final int kLeftFrontModuleAzimuth = 0;
-    public static final int kLeftFrontModuleEncoder = 0;
-
-    public static final int kRightFrontModuleDrive = 0;
-    public static final int kRightFrontModuleAzimuth = 0;
-    public static final int kRightFrontModuleEncoder = 0;
-
-    public static final int kLeftBackModuleDrive = 0;
-    public static final int kLeftBackModuleAzimuth = 0;
-    public static final int kLeftBackModuleEncoder = 0;
-
-    public static final int kRightBackModuleDrive = 0;
-    public static final int kRightBackModuleAzimuth = 0;
-    public static final int kRightBackModuleEncoder = 0;
-
-    public static final int kLeftTurretFlywheel = 0;
-    public static final int kLeftTurretHood = 0;
-    public static final int kLeftTurretAzimuth = 0;
-
-    public static final int kRightTurretFlywheel = 0;
-    public static final int kRightTurretHood = 0;
-    public static final int kRightTurretAzimuth = 0;
-
-    public static final int kLeftGuts = 0;
-    public static final int kRightGuts = 0;
-
-    public static final int kIntakeDrive = 0;
-    public static final int kIntakePivot = 0;
+    public static final int kPigeon2 =
+        DriveConstants.TunerConstants.DrivetrainConstants.Pigeon2Id; // 23
+
+    public static final int kFrontLeftModuleDrive =
+        DriveConstants.TunerConstants.FrontLeft.DriveMotorId; // 1
+    public static final int kFrontLeftModuleAzimuth =
+        DriveConstants.TunerConstants.FrontLeft.SteerMotorId; // 2
+    public static final int kFrontLeftModuleEncoder =
+        DriveConstants.TunerConstants.FrontLeft.EncoderId; // 19
+
+    public static final int kFrontRightModuleDrive =
+        DriveConstants.TunerConstants.FrontRight.DriveMotorId; // 3
+    public static final int kFrontRightModuleAzimuth =
+        DriveConstants.TunerConstants.FrontRight.SteerMotorId; // 4
+    public static final int kFrontRightModuleEncoder =
+        DriveConstants.TunerConstants.FrontRight.EncoderId; // 20
+
+    public static final int kBackLeftModuleDrive =
+        DriveConstants.TunerConstants.BackLeft.DriveMotorId; // 5
+    public static final int kBackLeftModuleAzimuth =
+        DriveConstants.TunerConstants.BackLeft.SteerMotorId; // 6
+    public static final int kBackLeftModuleEncoder =
+        DriveConstants.TunerConstants.BackLeft.EncoderId; // 21
+
+    public static final int kBackRightModuleDrive =
+        DriveConstants.TunerConstants.BackRight.DriveMotorId; // 7
+    public static final int kBackRightModuleAzimuth =
+        DriveConstants.TunerConstants.BackRight.SteerMotorId; // 8
+    public static final int kBackRightModuleEncoder =
+        DriveConstants.TunerConstants.BackRight.EncoderId; // 22
+
+    public static final int kLeftTurretFlywheel = 9;
+    public static final int kLeftTurretHood = 10;
+    public static final int kLeftTurretAzimuth = 11;
+
+    public static final int kRightTurretFlywheel = 12;
+    public static final int kRightTurretHood = 13;
+    public static final int kRightTurretAzimuth = 14;
+
+    public static final int kLeftGuts = 15;
+    public static final int kRightGuts = 16;
+
+    public static final int kIntakeDrive = 17;
+    public static final int kIntakePivot = 18;
   }
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 2e9a63e..03af959 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -8,18 +8,19 @@
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.config.PIDConstants;
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.RobotState.OdometryObservation;
-import frc.robot.commands.DriveCommands;
+import frc.robot.control.Configurable;
+import frc.robot.control.DefaultControls;
+import frc.robot.control.DriverController;
+import frc.robot.control.DriverControls;
 import frc.robot.subsystems.drive.Drive;
-import frc.robot.subsystems.drive.DriveConstants.ModuleConstants;
+import frc.robot.subsystems.drive.DriveConstants.TunerConstants;
 import frc.robot.subsystems.drive.GyroIO;
 import frc.robot.subsystems.drive.GyroIOPigeon2;
 import frc.robot.subsystems.drive.ModuleIO;
@@ -29,19 +30,19 @@
 import frc.robot.subsystems.guts.Guts.GutSide;
 import frc.robot.subsystems.guts.GutsIOSim;
 import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.intake.IntakeIOSim;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.Shooter.ShooterSide;
 import frc.robot.subsystems.shooter.flywheel.FlywheelIOSim;
 import frc.robot.subsystems.shooter.hood.HoodIOSim;
 import frc.robot.subsystems.shooter.turret.TurretIOSim;
 import frc.robot.util.AllianceFlipUtil;
-import frc.robot.util.Direction;
 import frc.robot.util.FieldConstants;
-import frc.robot.util.FieldConstants.Hub;
+import java.util.List;
 
 public class RobotContainer {
-  private final CommandXboxController driver =
-      new CommandXboxController(Constants.kDriverControllerPort);
+  private final DriverController driver = new DriverController.XboxDriverController(0);
+  private final Joystick operator = new Joystick(Constants.kOperatorControllerPort);
 
   private Drive drive;
   private Shooter leftShooter;
@@ -57,20 +58,35 @@ public RobotContainer() {
         drive =
             new Drive(
                 new GyroIOPigeon2(),
-                new ModuleIOTalonFX(ModuleConstants.FrontLeft),
-                new ModuleIOTalonFX(ModuleConstants.FrontRight),
-                new ModuleIOTalonFX(ModuleConstants.BackLeft),
-                new ModuleIOTalonFX(ModuleConstants.BackRight));
+                new ModuleIOTalonFX(TunerConstants.FrontLeft),
+                new ModuleIOTalonFX(TunerConstants.FrontRight),
+                new ModuleIOTalonFX(TunerConstants.BackLeft),
+                new ModuleIOTalonFX(TunerConstants.BackRight));
         // vision = new Vision(null, null);
+        // leftShooter =
+        //     new Shooter(
+        //         ShooterSide.LEFT,
+        //         new TurretIOSparkMax(DeviceIDs.kLeftTurretAzimuth),
+        //         new HoodIOSparkMax(DeviceIDs.kLeftTurretHood),
+        //         new FlywheelIOTalonFX(DeviceIDs.kLeftTurretFlywheel));
+        // rightShooter =
+        //     new Shooter(
+        //         ShooterSide.RIGHT,
+        //         new TurretIOSparkMax(DeviceIDs.kRightTurretAzimuth),
+        //         new HoodIOSparkMax(DeviceIDs.kRightTurretHood),
+        //         new FlywheelIOTalonFX(DeviceIDs.kRightTurretFlywheel));
+        // leftGuts = new Guts(GutSide.LEFT, new GutsIOSparkMax(DeviceIDs.kLeftGuts));
+        // rightGuts = new Guts(GutSide.RIGHT, new GutsIOSparkMax(DeviceIDs.kRightGuts));
+        // intake = new Intake(new IntakeIOHardware());
         break;
       case SIM:
         drive =
             new Drive(
                 new GyroIO() {},
-                new ModuleIOSim(ModuleConstants.FrontLeft),
-                new ModuleIOSim(ModuleConstants.FrontRight),
-                new ModuleIOSim(ModuleConstants.BackLeft),
-                new ModuleIOSim(ModuleConstants.BackRight));
+                new ModuleIOSim(TunerConstants.FrontLeft),
+                new ModuleIOSim(TunerConstants.FrontRight),
+                new ModuleIOSim(TunerConstants.BackLeft),
+                new ModuleIOSim(TunerConstants.BackRight));
         leftShooter =
             new Shooter(ShooterSide.LEFT, new TurretIOSim(), new HoodIOSim(), new FlywheelIOSim());
         rightShooter =
@@ -79,13 +95,14 @@ public RobotContainer() {
         drive =
             new Drive(
                 new GyroIO() {},
-                new ModuleIOSim(ModuleConstants.FrontLeft),
-                new ModuleIOSim(ModuleConstants.FrontRight),
-                new ModuleIOSim(ModuleConstants.BackLeft),
-                new ModuleIOSim(ModuleConstants.BackRight));
+                new ModuleIOSim(TunerConstants.FrontLeft),
+                new ModuleIOSim(TunerConstants.FrontRight),
+                new ModuleIOSim(TunerConstants.BackLeft),
+                new ModuleIOSim(TunerConstants.BackRight));
         // vision = new Vision(null, null);
         leftGuts = new Guts(GutSide.LEFT, new GutsIOSim());
         rightGuts = new Guts(GutSide.RIGHT, new GutsIOSim());
+        intake = new Intake(new IntakeIOSim());
 
         break;
       case REPLAY:
@@ -97,6 +114,8 @@ public RobotContainer() {
                 new ModuleIO() {},
                 new ModuleIO() {},
                 new ModuleIO() {});
+        leftShooter = new Shooter(null, null, null, null);
+        rightShooter = new Shooter(null, null, null, null);
         // vision = new Vision(null, new CameraIO[] {});
         break;
     }
@@ -115,51 +134,14 @@ public RobotContainer() {
   }
 
   private void configureBindings() {
-    drive.setDefaultCommand(
-        DriveCommands.joystickDrive(
-            drive, () -> -driver.getLeftY(), () -> -driver.getLeftX(), () -> -driver.getRightX()));
-    leftShooter.setDefaultCommand(
-        leftShooter.trackTarget(
-            () -> AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d())));
-    rightShooter.setDefaultCommand(
-        rightShooter.trackTarget(
-            () -> AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d())));
-
-    driver.povUp().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTH));
-    driver.povUpRight().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTHEAST));
-    driver.povRight().whileTrue(DriveCommands.crabWalk(drive, Direction.EAST));
-    driver.povDownRight().whileTrue(DriveCommands.crabWalk(drive, Direction.SOUTHEAST));
-    driver.povDown().whileTrue(DriveCommands.crabWalk(drive, Direction.SOUTH));
-    driver.povDownLeft().whileTrue(DriveCommands.crabWalk(drive, Direction.SOUTHWEST));
-    driver.povLeft().whileTrue(DriveCommands.crabWalk(drive, Direction.WEST));
-    driver.povUpLeft().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTHWEST));
-
-    driver.rightBumper().whileTrue(Shooter.shootBothAtHub(leftShooter, rightShooter));
-
-    driver
-        .a()
-        .whileTrue(
-            DriveCommands.joystickDriveAtAngle(
-                drive,
-                () -> -driver.getLeftY(), // xSupplier
-                () -> -driver.getLeftX(), // ySupplier
-                () -> {
-                  Pose2d robotPose = RobotState.getInstance().getEstimatedPose();
-                  Translation2d target =
-                      AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d());
-
-                  Translation2d delta = target.minus(robotPose.getTranslation());
-
-                  return new Rotation2d(Math.atan2(delta.getY(), delta.getX()));
-                }));
-
-    driver
-        .y()
-        .onTrue(
-            DriveCommands.turnToPoint(
-                drive,
-                () -> RobotState.getInstance().getEstimatedPose(),
-                () -> Hub.innerCenterPoint.toTranslation2d()));
+    List.of(
+            new DefaultControls(driver, operator, drive, leftShooter, rightShooter),
+            new DriverControls(
+                driver, operator, drive, leftShooter, rightShooter, leftGuts, rightGuts, intake)
+            //         // TODO: Implement ZoneControls
+            //         // ,new ZoneControls()
+            )
+        .forEach(Configurable::configure);
   }
 
   public void robotPeriodic() {
@@ -167,6 +149,10 @@ public void robotPeriodic() {
         new OdometryObservation(
             Timer.getTimestamp(), drive.getModulePositions(), drive.getRawGyroRotation());
     RobotState.getInstance().addOdometryObservation(obs);
+    RobotState.getInstance().setRobotVelocity(drive.getChassisSpeeds());
+    // System.out.println(RobotState.getInstance().getEstimatedPose().getX());
+    // System.out.println(RobotState.getInstance().getRobotVelocity().vxMetersPerSecond);
+    // System.out.println(driver.getLeftX());
   }
 
   public Command getAutonomousCommand() {
diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java
index 00e1eca..44b28c6 100644
--- a/src/main/java/frc/robot/RobotState.java
+++ b/src/main/java/frc/robot/RobotState.java
@@ -23,7 +23,9 @@ public static RobotState getInstance() {
   /** Pose Estimator */
   private SwerveDrivePoseEstimator poseEstimator;
 
-  private ChassisSpeeds robotVelocity;
+  private ChassisSpeeds robotVelocity = new ChassisSpeeds();
+
+  private Rotation2d gyroOffset = new Rotation2d();
 
   private RobotState() {
     poseEstimator =
@@ -92,11 +94,11 @@ public void setPose(Pose2d pose) {
   }
 
   public void resetRotation(Rotation2d rotation) {
-    poseEstimator.resetRotation(rotation);
+    gyroOffset = poseEstimator.getEstimatedPosition().getRotation().minus(rotation);
   }
 
   /**
-   * Set the robot's velocity
+   * Set the robot's velocity.
    *
    * @param speeds A ChassisSpeeds object representing the robot's current velocity
    */
@@ -124,7 +126,7 @@ public ChassisSpeeds getRobotVelocity() {
 
   /** Get the rotation of the estimated pose. */
   public Rotation2d getRotation() {
-    return poseEstimator.getEstimatedPosition().getRotation();
+    return poseEstimator.getEstimatedPosition().getRotation().minus(gyroOffset);
   }
 
   public ChassisSpeeds getFieldVelocity() {
diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java
index a26f9de..501e5e0 100644
--- a/src/main/java/frc/robot/commands/DriveCommands.java
+++ b/src/main/java/frc/robot/commands/DriveCommands.java
@@ -177,26 +177,10 @@ public static Command turnToPoint(
   public static Command crabWalk(Drive drive, Direction direction) {
     return drive.run(
         () -> {
+          // TODO: tune ts
           double speed = 1.0; // meters per second (tune this)
 
-          // Convert enum to vector
-          double dx = direction.getDx();
-          double dy = direction.getDy();
-
-          // Normalize so diagonals aren't faster
-          double magnitude = Math.hypot(dx, dy);
-          if (magnitude > 0) {
-            dx /= magnitude;
-            dy /= magnitude;
-          }
-
-          // Build chassis speeds (no rotation)
-          ChassisSpeeds speeds =
-              new ChassisSpeeds(
-                  dx * speed, // vx (forward)
-                  dy * speed, // vy (left)
-                  0.0 // omega (no turning)
-                  );
+          ChassisSpeeds speeds = direction.toChassisSpeeds().times(speed);
 
           drive.runVelocity(speeds);
         });
diff --git a/src/main/java/frc/robot/control/DefaultControls.java b/src/main/java/frc/robot/control/DefaultControls.java
index 470985f..07b7103 100644
--- a/src/main/java/frc/robot/control/DefaultControls.java
+++ b/src/main/java/frc/robot/control/DefaultControls.java
@@ -30,6 +30,8 @@ public DefaultControls(
   /** Configure all default commands for the subsystems (e.g. includes joystick driving). */
   @Override
   public void configure() {
-    drive.setDefaultCommand(DriveCommands.joystickDrive(drive, null, null, null));
+    drive.setDefaultCommand(
+        DriveCommands.joystickDrive(
+            drive, () -> driver.getLeftY(), () -> driver.getLeftX(), () -> -driver.getRightX()));
   }
 }
diff --git a/src/main/java/frc/robot/control/DriverController.java b/src/main/java/frc/robot/control/DriverController.java
index 74de15a..25df11a 100644
--- a/src/main/java/frc/robot/control/DriverController.java
+++ b/src/main/java/frc/robot/control/DriverController.java
@@ -1,5 +1,6 @@
 package frc.robot.control;
 
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -50,11 +51,13 @@ public interface DriverController {
 
   double getRightY();
 
+  void rumble(RumbleType rumbleType, double intensity);
+
   class XboxDriverController implements DriverController {
     private final CommandXboxController controller;
 
-    public XboxDriverController(CommandXboxController controller) {
-      this.controller = controller;
+    public XboxDriverController(int controllerID) {
+      this.controller = new CommandXboxController(controllerID);
     }
 
     @Override
@@ -156,13 +159,18 @@ public double getRightX() {
     public double getRightY() {
       return controller.getRightY();
     }
+
+    @Override
+    public void rumble(RumbleType rumbleType, double intensity) {
+      controller.setRumble(rumbleType, intensity);
+    }
   }
 
   class PS5DriverController implements DriverController {
     private final CommandPS5Controller controller;
 
-    public PS5DriverController(CommandPS5Controller controller) {
-      this.controller = controller;
+    public PS5DriverController(int controllerID) {
+      this.controller = new CommandPS5Controller(controllerID);
     }
 
     @Override
@@ -264,5 +272,10 @@ public double getRightX() {
     public double getRightY() {
       return controller.getRightY();
     }
+
+    @Override
+    public void rumble(RumbleType rumbleType, double intensity) {
+      controller.setRumble(rumbleType, intensity);
+    }
   }
 }
diff --git a/src/main/java/frc/robot/control/DriverControls.java b/src/main/java/frc/robot/control/DriverControls.java
index 597782b..cf7aa17 100644
--- a/src/main/java/frc/robot/control/DriverControls.java
+++ b/src/main/java/frc/robot/control/DriverControls.java
@@ -1,8 +1,14 @@
 package frc.robot.control;
 
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.commands.DriveCommands;
 import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.guts.Guts;
+import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.util.Direction;
 import org.littletonrobotics.junction.AutoLogOutput;
 
 public class DriverControls implements Configurable {
@@ -19,25 +25,51 @@ public enum DriverMode {
   private final Drive drive;
   private final Shooter leftShooter;
   private final Shooter rightShooter;
+  private final Guts leftGuts;
+  private final Guts rightGuts;
+  private final Intake intake;
 
   public DriverControls(
       DriverController driver,
       Joystick operator,
       Drive drive,
       Shooter leftShooter,
-      Shooter rightShooter) {
+      Shooter rightShooter,
+      Guts leftGuts,
+      Guts rightGuts,
+      Intake intake) {
     this.driver = driver;
     this.operator = operator;
     this.drive = drive;
     this.leftShooter = leftShooter;
     this.rightShooter = rightShooter;
+    this.leftGuts = leftGuts;
+    this.rightGuts = rightGuts;
+    this.intake = intake;
   }
 
   @Override
   public void configure() {
 
     // Neutral controls (regardless of whether we are in one or two driver mode)
-    driver.xSquare().onTrue(Shooter.shootBothAtHub(leftShooter, rightShooter));
+    driver.xSquare().onTrue(Commands.runOnce(drive::zeroYaw));
+
+    driver
+        .bCircle()
+        .onTrue(
+            Commands.runEnd(
+                    () -> driver.rumble(RumbleType.kBothRumble, 1),
+                    () -> driver.rumble(RumbleType.kBothRumble, 0.0))
+                .withTimeout(0.25));
+
+    driver.dPadUp().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTH));
+    driver.dPadUpLeft().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTHWEST));
+    driver.dPadUpRight().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTHEAST));
+    driver.dPadLeft().whileTrue(DriveCommands.crabWalk(drive, Direction.WEST));
+    driver.dPadRight().whileTrue(DriveCommands.crabWalk(drive, Direction.EAST));
+    driver.dPadDownLeft().whileTrue(DriveCommands.crabWalk(drive, Direction.SOUTHWEST));
+    driver.dPadDownRight().whileTrue(DriveCommands.crabWalk(drive, Direction.SOUTHEAST));
+    driver.dPadDown().whileTrue(DriveCommands.crabWalk(drive, Direction.SOUTH));
 
     configureOneDriver();
     configureTwoDrivers();
@@ -53,10 +85,19 @@ public void configure() {
    */
 
   private void configureOneDriver() {
-    driver
-        .rightBumper()
-        .and(this::isOneDriver)
-        .onTrue(Shooter.shootBothAtHub(leftShooter, rightShooter));
+
+    // driver
+    //     .rightBumper()
+    //     .and(this::isOneDriver)
+    //     .onTrue(Shooter.shootBothAtHub(leftShooter, rightShooter));
+
+    // driver.leftBumper().and(this::isOneDriver).onTrue(intake.deploy().withTimeout(0.5));
+
+    // driver.leftBumper().and(this::isOneDriver).onTrue(intake.retract().withTimeout(0.5));
+
+    // driver.leftTrigger().and(this::isOneDriver).whileTrue(intake.intake());
+
+    // driver.leftTrigger().and(this::isOneDriver).whileTrue(intake.outtake());
   }
 
   /*
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
index 236f5b7..490e2bf 100644
--- a/src/main/java/frc/robot/subsystems/drive/Drive.java
+++ b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -27,9 +27,7 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
 import frc.robot.Constants.Mode;
-import frc.robot.RobotState;
-import frc.robot.RobotState.OdometryObservation;
-import frc.robot.subsystems.drive.DriveConstants.ModuleConstants;
+import frc.robot.subsystems.drive.DriveConstants.TunerConstants;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
 import org.littletonrobotics.junction.AutoLogOutput;
@@ -62,10 +60,10 @@ public Drive(
       ModuleIO blModuleIO,
       ModuleIO brModuleIO) {
     this.gyroIO = gyroIO;
-    modules[0] = new Module(flModuleIO, 0, ModuleConstants.FrontLeft);
-    modules[1] = new Module(frModuleIO, 1, ModuleConstants.FrontRight);
-    modules[2] = new Module(blModuleIO, 2, ModuleConstants.BackLeft);
-    modules[3] = new Module(brModuleIO, 3, ModuleConstants.BackRight);
+    modules[0] = new Module(flModuleIO, 0, TunerConstants.FrontLeft);
+    modules[1] = new Module(frModuleIO, 1, TunerConstants.FrontRight);
+    modules[2] = new Module(blModuleIO, 2, TunerConstants.BackLeft);
+    modules[3] = new Module(brModuleIO, 3, TunerConstants.BackRight);
 
     // Usage reporting for swerve template
     HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDriveSwerve_AdvantageKit);
@@ -123,7 +121,9 @@ public void periodic() {
                 modulePositions[moduleIndex].distanceMeters
                     - lastModulePositions[moduleIndex].distanceMeters,
                 modulePositions[moduleIndex].angle);
-        lastModulePositions[moduleIndex] = modulePositions[moduleIndex];
+        lastModulePositions[moduleIndex] =
+            new SwerveModulePosition(
+                modulePositions[moduleIndex].distanceMeters, modulePositions[moduleIndex].angle);
       }
 
       // Update gyro angle
@@ -136,11 +136,12 @@ public void periodic() {
         rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta));
       }
 
-      // Apply update
-      RobotState.getInstance()
-          .addOdometryObservation(
-              new OdometryObservation(sampleTimestamps[i], modulePositions, rawGyroRotation));
-      RobotState.getInstance().setRobotVelocity(getChassisSpeeds());
+      // Apply update (doesn't work)
+      // RobotState.getInstance()
+      //     .addOdometryObservation(
+      //         new OdometryObservation(sampleTimestamps[i], modulePositions, rawGyroRotation));
+
+      Logger.recordOutput("Drive/MeasuredPositions", modulePositions);
     }
 
     // Update gyro alert
@@ -156,7 +157,7 @@ public void runVelocity(ChassisSpeeds speeds) {
     // Calculate module setpoints
     ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02);
     SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds);
-    SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, ModuleConstants.kSpeedAt12Volts);
+    SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, TunerConstants.kSpeedAt12Volts);
 
     // Log unoptimized setpoints and setpoint speeds
     Logger.recordOutput("SwerveStates/Setpoints", setpointStates);
@@ -178,6 +179,13 @@ public void runCharacterization(double output) {
     }
   }
 
+  /** Runs all modules' drive motor at the specified output */
+  public void runDriveOpenLoop(double output) {
+    for (int i = 0; i < 4; i++) {
+      modules[i].runDriveOpenLoop(output);
+    }
+  }
+
   /** Stops the drive. */
   public void stop() {
     runVelocity(new ChassisSpeeds());
@@ -196,6 +204,21 @@ public void stopWithX() {
     stop();
   }
 
+  /**
+   * Sets the gyro yaw to the specified angle.
+   *
+   * @param angle The angle to set the gyro to.
+   */
+  public void setYaw(Rotation2d angle) {
+    gyroIO.setYaw(angle);
+    rawGyroRotation = angle;
+  }
+
+  /** Zeros the gyro yaw. */
+  public void zeroYaw() {
+    setYaw(Rotation2d.kZero);
+  }
+
   /** Returns a command to run a quasistatic test in the specified direction. */
   public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
     return run(() -> runCharacterization(0.0))
@@ -229,7 +252,7 @@ public SwerveModulePosition[] getModulePositions() {
 
   /** Returns the measured chassis speeds of the robot. */
   @AutoLogOutput(key = "SwerveChassisSpeeds/Measured")
-  private ChassisSpeeds getChassisSpeeds() {
+  public ChassisSpeeds getChassisSpeeds() {
     return kinematics.toChassisSpeeds(getModuleStates());
   }
 
@@ -258,7 +281,7 @@ public Rotation2d getRawGyroRotation() {
 
   /** Returns the maximum linear speed in meters per sec. */
   public double getMaxLinearSpeedMetersPerSec() {
-    return ModuleConstants.kSpeedAt12Volts.in(MetersPerSecond);
+    return TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
   }
 
   /** Returns the maximum angular speed in radians per sec. */
@@ -269,10 +292,10 @@ public double getMaxAngularSpeedRadPerSec() {
   /** Returns an array of module translations. */
   public static Translation2d[] getModuleTranslations() {
     return new Translation2d[] {
-      new Translation2d(ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY),
-      new Translation2d(ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY),
-      new Translation2d(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY),
-      new Translation2d(ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY)
+      new Translation2d(TunerConstants.FrontLeft.LocationX, TunerConstants.FrontLeft.LocationY),
+      new Translation2d(TunerConstants.FrontRight.LocationX, TunerConstants.FrontRight.LocationY),
+      new Translation2d(TunerConstants.BackLeft.LocationX, TunerConstants.BackLeft.LocationY),
+      new Translation2d(TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY)
     };
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
index cdb8d5d..54a22f2 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -1,6 +1,7 @@
 package frc.robot.subsystems.drive;
 
 import static edu.wpi.first.units.Units.Amps;
+import static edu.wpi.first.units.Units.Degrees;
 import static edu.wpi.first.units.Units.Inches;
 import static edu.wpi.first.units.Units.KilogramSquareMeters;
 import static edu.wpi.first.units.Units.MetersPerSecond;
@@ -10,6 +11,7 @@
 import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MountPoseConfigs;
 import com.ctre.phoenix6.configs.Pigeon2Configuration;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
@@ -38,25 +40,22 @@ public final class DriveConstants {
       new SwerveDriveKinematics(Drive.getModuleTranslations());
 
   public static final double kOdometryFrequency =
-      ModuleConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0;
+      TunerConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0;
   public static final double kDriveBaseRadius =
       Math.max(
           Math.max(
-              Math.hypot(ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY),
-              Math.hypot(
-                  ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY)),
+              Math.hypot(TunerConstants.FrontLeft.LocationX, TunerConstants.FrontLeft.LocationY),
+              Math.hypot(TunerConstants.FrontRight.LocationX, TunerConstants.FrontRight.LocationY)),
           Math.max(
-              Math.hypot(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY),
-              Math.hypot(
-                  ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY)));
+              Math.hypot(TunerConstants.BackLeft.LocationX, TunerConstants.BackLeft.LocationY),
+              Math.hypot(TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY)));
 
   public static final Translation2d[] kModuleTranslations =
       new Translation2d[] {
-        new Translation2d(ModuleConstants.FrontLeft.LocationX, ModuleConstants.FrontLeft.LocationY),
-        new Translation2d(
-            ModuleConstants.FrontRight.LocationX, ModuleConstants.FrontRight.LocationY),
-        new Translation2d(ModuleConstants.BackLeft.LocationX, ModuleConstants.BackLeft.LocationY),
-        new Translation2d(ModuleConstants.BackRight.LocationX, ModuleConstants.BackRight.LocationY)
+        new Translation2d(TunerConstants.FrontLeft.LocationX, TunerConstants.FrontLeft.LocationY),
+        new Translation2d(TunerConstants.FrontRight.LocationX, TunerConstants.FrontRight.LocationY),
+        new Translation2d(TunerConstants.BackLeft.LocationX, TunerConstants.BackLeft.LocationY),
+        new Translation2d(TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY)
       };
 
   // TODO: Update for robot
@@ -71,33 +70,33 @@ public final class DriveConstants {
           kRobotMOI,
           kRobotMOI,
           new ModuleConfig(
-              ModuleConstants.FrontLeft.WheelRadius,
-              ModuleConstants.kSpeedAt12Volts.in(MetersPerSecond),
+              TunerConstants.FrontLeft.WheelRadius,
+              TunerConstants.kSpeedAt12Volts.in(MetersPerSecond),
               kWheelCOF,
-              DCMotor.getKrakenX60(1).withReduction(ModuleConstants.FrontLeft.DriveMotorGearRatio),
-              ModuleConstants.FrontLeft.SlipCurrent,
+              DCMotor.getKrakenX60(1).withReduction(TunerConstants.FrontLeft.DriveMotorGearRatio),
+              TunerConstants.FrontLeft.SlipCurrent,
               1),
           kModuleTranslations);
 
-  public static final class ModuleConstants {
-    // Both sets of gains need to be tuned to your individual robot.
+  // Generated by the 2026 Tuner X Swerve Project Generator
+  // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
+  public class TunerConstants {
+    // TODO: Both sets of gains need to be tuned to your individual robot.
 
     // The steer motor uses any SwerveModule.SteerRequestType control request with
     // the
     // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
-    // TODO: Update for robot
     private static final Slot0Configs steerGains =
         new Slot0Configs()
             .withKP(100)
             .withKI(0)
             .withKD(0.5)
             .withKS(0.1)
-            .withKV(1.91)
+            .withKV(2.49)
             .withKA(0)
             .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
     // When using closed-loop control, the drive motor uses the control
     // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
-    // TODO: Update for robot
     private static final Slot0Configs driveGains =
         new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124);
 
@@ -116,14 +115,12 @@ public static final class ModuleConstants {
         SteerMotorArrangement.TalonFX_Integrated;
 
     // The remote sensor feedback type to use for the steer motors;
-    // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to
-    // RemoteCANcoder
+    // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote*
     private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
 
     // The stator current at which the wheels start to slip;
     // This needs to be tuned to your individual robot
-    // TODO: Update for robot
-    private static final Current kSlipCurrent = Amps.of(120.0);
+    private static final Current kSlipCurrent = Amps.of(120);
 
     // Initial configs for the drive and steer motors and the azimuth encoder; these
     // cannot be null.
@@ -135,41 +132,40 @@ public static final class ModuleConstants {
             .withCurrentLimits(
                 new CurrentLimitsConfigs()
                     // Swerve azimuth does not require much torque output, so we can set a
-                    // relatively
-                    // low
+                    // relatively low
                     // stator current limit to help avoid brownouts without impacting performance.
                     .withStatorCurrentLimit(Amps.of(60))
                     .withStatorCurrentLimitEnable(true));
     private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
     // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
-    private static final Pigeon2Configuration pigeonConfigs = null;
+    private static final Pigeon2Configuration pigeonConfigs =
+        new Pigeon2Configuration()
+            .withMountPose(new MountPoseConfigs().withMountPoseYaw(Degrees.of(-180)));
 
     // CAN bus that the devices are located on;
     // All swerve devices must share the same CAN bus
-    public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot");
+    public static final CANBus kCANBus = new CANBus("", "./logs/example.hoot");
 
     // Theoretical free speed (m/s) at 12 V applied output;
     // This needs to be tuned to your individual robot
-    // TODO: Update for robot
-    public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69);
+    public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.85);
 
     // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
     // This may need to be tuned to your individual robot
-    // TODO: Update for robot
-    private static final double kCoupleRatio = 3.8181818181818183;
-    // TODO: Update for robot
-    private static final double kDriveGearRatio = 7.363636363636365;
-    private static final double kSteerGearRatio = 15.42857142857143;
-    private static final Distance kWheelRadius = Inches.of(2.167);
-    // TODO: Update for robot
+    private static final double kCoupleRatio = 3.375;
+
+    private static final double kDriveGearRatio = 5.2734375;
+    private static final double kSteerGearRatio = 26.09090909090909;
+    private static final Distance kWheelRadius = Inches.of(2);
+
     private static final boolean kInvertLeftSide = false;
     private static final boolean kInvertRightSide = true;
-    // TODO: Update for robot
-    private static final int kPigeonId = 1;
+
+    private static final int kPigeonId = 23;
 
     // These are only used for simulation
-    private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004);
-    private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025);
+    private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01);
+    private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01);
     // Simulated voltage necessary to overcome friction
     private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
     private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
@@ -206,50 +202,49 @@ public static final class ModuleConstants {
                 .withSteerFrictionVoltage(kSteerFrictionVoltage)
                 .withDriveFrictionVoltage(kDriveFrictionVoltage);
 
-    // TODO: Update for robot
     // Front Left
-    private static final int kFrontLeftDriveMotorId = 3;
+    private static final int kFrontLeftDriveMotorId = 1;
     private static final int kFrontLeftSteerMotorId = 2;
-    private static final int kFrontLeftEncoderId = 1;
-    private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375);
-    private static final boolean kFrontLeftSteerMotorInverted = true;
+    private static final int kFrontLeftEncoderId = 19;
+    private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.282470703125);
+    private static final boolean kFrontLeftSteerMotorInverted = false;
     private static final boolean kFrontLeftEncoderInverted = false;
 
-    private static final Distance kFrontLeftXPos = Inches.of(10);
-    private static final Distance kFrontLeftYPos = Inches.of(10);
-    // TODO: Update for robot
+    private static final Distance kFrontLeftXPos = Inches.of(9.375);
+    private static final Distance kFrontLeftYPos = Inches.of(12.875);
+
     // Front Right
-    private static final int kFrontRightDriveMotorId = 1;
-    private static final int kFrontRightSteerMotorId = 0;
-    private static final int kFrontRightEncoderId = 0;
-    private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875);
-    private static final boolean kFrontRightSteerMotorInverted = true;
+    private static final int kFrontRightDriveMotorId = 3;
+    private static final int kFrontRightSteerMotorId = 4;
+    private static final int kFrontRightEncoderId = 20;
+    private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.311767578125);
+    private static final boolean kFrontRightSteerMotorInverted = false;
     private static final boolean kFrontRightEncoderInverted = false;
 
-    private static final Distance kFrontRightXPos = Inches.of(10);
-    private static final Distance kFrontRightYPos = Inches.of(-10);
-    // TODO: Update for robot
+    private static final Distance kFrontRightXPos = Inches.of(9.375);
+    private static final Distance kFrontRightYPos = Inches.of(-12.875);
+
     // Back Left
-    private static final int kBackLeftDriveMotorId = 7;
+    private static final int kBackLeftDriveMotorId = 5;
     private static final int kBackLeftSteerMotorId = 6;
-    private static final int kBackLeftEncoderId = 3;
-    private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875);
-    private static final boolean kBackLeftSteerMotorInverted = true;
+    private static final int kBackLeftEncoderId = 21;
+    private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.05712890625);
+    private static final boolean kBackLeftSteerMotorInverted = false;
     private static final boolean kBackLeftEncoderInverted = false;
 
-    private static final Distance kBackLeftXPos = Inches.of(-10);
-    private static final Distance kBackLeftYPos = Inches.of(10);
-    // TODO: Update for robot
+    private static final Distance kBackLeftXPos = Inches.of(-9.375);
+    private static final Distance kBackLeftYPos = Inches.of(12.875);
+
     // Back Right
-    private static final int kBackRightDriveMotorId = 5;
-    private static final int kBackRightSteerMotorId = 4;
-    private static final int kBackRightEncoderId = 2;
-    private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125);
-    private static final boolean kBackRightSteerMotorInverted = true;
+    private static final int kBackRightDriveMotorId = 7;
+    private static final int kBackRightSteerMotorId = 8;
+    private static final int kBackRightEncoderId = 22;
+    private static final Angle kBackRightEncoderOffset = Rotations.of(0.193603515625);
+    private static final boolean kBackRightSteerMotorInverted = false;
     private static final boolean kBackRightEncoderInverted = false;
 
-    private static final Distance kBackRightXPos = Inches.of(-10);
-    private static final Distance kBackRightYPos = Inches.of(-10);
+    private static final Distance kBackRightXPos = Inches.of(-9.375);
+    private static final Distance kBackRightYPos = Inches.of(-12.875);
 
     public static final SwerveModuleConstants<
             TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java
index 4e9754f..910155c 100644
--- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java
+++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java
@@ -21,4 +21,6 @@ public static class GyroIOInputs {
   }
 
   public default void updateInputs(GyroIOInputs inputs) {}
+
+  public default void setYaw(Rotation2d angle) {}
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java
index 6236486..55d008c 100644
--- a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java
+++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java
@@ -40,4 +40,9 @@ public void updateInputs(GyroIOInputs inputs) {
     yawTimestampQueue.clear();
     yawPositionQueue.clear();
   }
+
+  @Override
+  public void setYaw(Rotation2d angle) {
+    navX.setAngleAdjustment(angle.getDegrees());
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
index 6a8f4ef..7f582a1 100644
--- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
+++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
@@ -16,21 +16,21 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
-import frc.robot.subsystems.drive.DriveConstants.ModuleConstants;
+import frc.robot.subsystems.drive.DriveConstants.TunerConstants;
 import java.util.Queue;
 
 /** IO implementation for Pigeon 2. */
 public class GyroIOPigeon2 implements GyroIO {
   private final Pigeon2 pigeon =
-      new Pigeon2(ModuleConstants.DrivetrainConstants.Pigeon2Id, ModuleConstants.kCANBus);
+      new Pigeon2(TunerConstants.DrivetrainConstants.Pigeon2Id, TunerConstants.kCANBus);
   private final StatusSignal yaw = pigeon.getYaw();
   private final Queue yawPositionQueue;
   private final Queue yawTimestampQueue;
   private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld();
 
   public GyroIOPigeon2() {
-    if (ModuleConstants.DrivetrainConstants.Pigeon2Configs != null) {
-      pigeon.getConfigurator().apply(ModuleConstants.DrivetrainConstants.Pigeon2Configs);
+    if (TunerConstants.DrivetrainConstants.Pigeon2Configs != null) {
+      pigeon.getConfigurator().apply(TunerConstants.DrivetrainConstants.Pigeon2Configs);
     } else {
       pigeon.getConfigurator().apply(new Pigeon2Configuration());
     }
@@ -58,4 +58,9 @@ public void updateInputs(GyroIOInputs inputs) {
     yawTimestampQueue.clear();
     yawPositionQueue.clear();
   }
+
+  @Override
+  public void setYaw(Rotation2d angle) {
+    pigeon.setYaw(angle.getDegrees());
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java
index 8f9781f..401d2ac 100644
--- a/src/main/java/frc/robot/subsystems/drive/Module.java
+++ b/src/main/java/frc/robot/subsystems/drive/Module.java
@@ -82,6 +82,11 @@ public void runSetpoint(SwerveModuleState state) {
     io.setTurnPosition(state.angle);
   }
 
+  /** Runs the drive motor at the specified output */
+  public void runDriveOpenLoop(double output) {
+    io.setDriveOpenLoop(output);
+  }
+
   /** Runs the module with the specified output while controlling to zero degrees. */
   public void runCharacterization(double output) {
     io.setDriveOpenLoop(output);
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java
index dc06a39..8221f86 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java
@@ -106,7 +106,7 @@ public void updateInputs(ModuleIOInputs inputs) {
 
     // Update odometry inputs (50Hz because high-frequency odometry in sim doesn't
     // matter)
-    inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()};
+    inputs.odometryTimestamps = new double[] {Timer.getTimestamp()};
     inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad};
     inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition};
   }
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
index a9d861b..6d537fc 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
@@ -34,7 +34,7 @@
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.units.measure.Voltage;
-import frc.robot.subsystems.drive.DriveConstants.ModuleConstants;
+import frc.robot.subsystems.drive.DriveConstants.TunerConstants;
 import java.util.Queue;
 
 /**
@@ -95,9 +95,9 @@ public ModuleIOTalonFX(
       SwerveModuleConstants
           constants) {
     this.constants = constants;
-    driveTalon = new TalonFX(constants.DriveMotorId, ModuleConstants.kCANBus);
-    turnTalon = new TalonFX(constants.SteerMotorId, ModuleConstants.kCANBus);
-    cancoder = new CANcoder(constants.EncoderId, ModuleConstants.kCANBus);
+    driveTalon = new TalonFX(constants.DriveMotorId, TunerConstants.kCANBus);
+    turnTalon = new TalonFX(constants.SteerMotorId, TunerConstants.kCANBus);
+    cancoder = new CANcoder(constants.EncoderId, TunerConstants.kCANBus);
 
     // Configure drive motor
     var driveConfig = constants.DriveMotorInitialConfigs;
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java
index d2c5412..1fd9552 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java
@@ -32,7 +32,7 @@
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.units.measure.Voltage;
-import frc.robot.subsystems.drive.DriveConstants.ModuleConstants;
+import frc.robot.subsystems.drive.DriveConstants.TunerConstants;
 import java.util.Queue;
 
 /**
@@ -81,9 +81,9 @@ public class ModuleIOTalonFXS implements ModuleIO {
   public ModuleIOTalonFXS(
       SwerveModuleConstants
           constants) {
-    driveTalon = new TalonFXS(constants.DriveMotorId, ModuleConstants.kCANBus);
-    turnTalon = new TalonFXS(constants.SteerMotorId, ModuleConstants.kCANBus);
-    candi = new CANdi(constants.EncoderId, ModuleConstants.kCANBus);
+    driveTalon = new TalonFXS(constants.DriveMotorId, TunerConstants.kCANBus);
+    turnTalon = new TalonFXS(constants.SteerMotorId, TunerConstants.kCANBus);
+    candi = new CANdi(constants.EncoderId, TunerConstants.kCANBus);
 
     // Configure drive motor
     var driveConfig = constants.DriveMotorInitialConfigs;
diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java
index 3a2b31d..6450ba2 100644
--- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java
+++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java
@@ -11,7 +11,7 @@
 import com.ctre.phoenix6.StatusSignal;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.RobotController;
-import frc.robot.subsystems.drive.DriveConstants.ModuleConstants;
+import frc.robot.subsystems.drive.DriveConstants.TunerConstants;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Queue;
@@ -37,7 +37,7 @@ public class PhoenixOdometryThread extends Thread {
   private final List> genericQueues = new ArrayList<>();
   private final List> timestampQueues = new ArrayList<>();
 
-  private static boolean isCANFD = ModuleConstants.kCANBus.isNetworkFD();
+  private static boolean isCANFD = TunerConstants.kCANBus.isNetworkFD();
   private static PhoenixOdometryThread instance = null;
 
   public static PhoenixOdometryThread getInstance() {
diff --git a/src/main/java/frc/robot/subsystems/guts/GutsConstants.java b/src/main/java/frc/robot/subsystems/guts/GutsConstants.java
index 37b7bd3..cfdbdcd 100644
--- a/src/main/java/frc/robot/subsystems/guts/GutsConstants.java
+++ b/src/main/java/frc/robot/subsystems/guts/GutsConstants.java
@@ -4,5 +4,5 @@ public final class GutsConstants {
 
   public static final double kGutMotorSpeed = 0.5;
   // Change Gear Ratio later
-  public static final double kGutMotorGearRatio = 0.0;
+  public static final double kGutMotorGearRatio = 1.0;
 }
diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java b/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java
index d450b8c..31e203f 100644
--- a/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java
@@ -20,10 +20,8 @@ public class GutsIOSparkMax implements GutsIO {
   private final SparkMax gutMotor;
   private final RelativeEncoder gutEncoder;
   private final SparkMaxConfig gutMotorConfig;
-  private final int motorID;
 
   public GutsIOSparkMax(int motorID) {
-    this.motorID = motorID;
     gutMotor = new SparkMax(motorID, MotorType.kBrushless);
     gutEncoder = gutMotor.getEncoder();
     gutMotorConfig = new SparkMaxConfig();
diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
index 002a794..e09517f 100644
--- a/src/main/java/frc/robot/subsystems/intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -24,7 +24,7 @@ public Intake(IntakeIO io) {
    *
    * @return runs the pivot at a speed on every iteration until end when it stops the running
    */
-  public Command runPivot() {
+  public Command deploy() {
     return Commands.runEnd(
         () -> io.setPivotSpeed(IntakeConstants.kPivotMotorSpeed),
         () -> io.setPivotSpeed(0.0),
@@ -36,9 +36,9 @@ public Command runPivot() {
    *
    * @return runs the pivot at a speed on every iteration until end when it stops the running
    */
-  public Command runPivotBack() {
+  public Command retract() {
     return Commands.runEnd(
-        () -> io.setPivotSpeed(-(IntakeConstants.kPivotMotorSpeed)),
+        () -> io.setPivotSpeed(-IntakeConstants.kPivotMotorSpeed),
         () -> io.setPivotSpeed(0.0),
         this);
   }
@@ -48,7 +48,7 @@ public Command runPivotBack() {
    *
    * @return runs the feeder at a speed on every iteration until end when it stops the running
    */
-  public Command runFeeder() {
+  public Command intake() {
     return Commands.runEnd(
         () -> io.setWheelSpeed(IntakeConstants.kRollerMotorSpeed),
         () -> io.setWheelSpeed(0.0),
@@ -60,7 +60,7 @@ public Command runFeeder() {
    *
    * @return runs the feeder at a speed on every iteration until end when it stops the running
    */
-  public Command runFeederBack() {
+  public Command outtake() {
     return Commands.runEnd(
         () -> io.setWheelSpeed(-(IntakeConstants.kRollerMotorSpeed)),
         () -> io.setWheelSpeed(0.0),
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
index 93ef3f4..5196340 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -1,9 +1,6 @@
 package frc.robot.subsystems.intake;
 
 public final class IntakeConstants {
-  public static final int kPivotMotorID = 8;
-  public static final int kRollerMotorID = 9;
-
   public static final double kPivotMotorSpeed = 0.5;
   public static final double kRollerMotorSpeed = 0.5;
 
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java
index b963bf7..1f33515 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java
@@ -9,17 +9,18 @@
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import edu.wpi.first.math.util.Units;
+import frc.robot.Constants.DeviceIDs;
 
 public class IntakeIOHardware implements IntakeIO {
-  private SparkMax pivotMotor = new SparkMax(IntakeConstants.kPivotMotorID, MotorType.kBrushless);
+  private SparkMax pivotMotor = new SparkMax(DeviceIDs.kIntakePivot, MotorType.kBrushless);
   private RelativeEncoder pivotEncoder = pivotMotor.getEncoder();
-  private TalonFX wheelMotor = new TalonFX(IntakeConstants.kRollerMotorID);
+  private TalonFX driveMotor = new TalonFX(DeviceIDs.kIntakeDrive);
   private SparkMaxConfig pivotConfig;
   private TalonFXConfiguration wheelMotorConfig;
 
   public IntakeIOHardware() {
     pivotConfig = new SparkMaxConfig();
-    wheelMotor.getConfigurator().apply(wheelMotorConfig);
+    driveMotor.getConfigurator().apply(wheelMotorConfig);
     pivotMotor.configure(
         pivotConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
   }
@@ -31,7 +32,7 @@ public void setPivotSpeed(double speed) {
 
   @Override
   public void setWheelSpeed(double speed) {
-    wheelMotor.set(speed);
+    driveMotor.set(speed);
   }
 
   @Override
@@ -39,12 +40,12 @@ public void updateInputs(IntakeIOInputs inputs) {
     inputs.pivotVelocityRadPerSec =
         Units.rotationsPerMinuteToRadiansPerSecond(pivotEncoder.getVelocity());
     inputs.wheelVelocityRadPerSec =
-        Units.rotationsToRadians(wheelMotor.getVelocity().getValueAsDouble());
+        Units.rotationsToRadians(driveMotor.getVelocity().getValueAsDouble());
     inputs.pivotPositionRad = Units.rotationsToRadians(pivotEncoder.getPosition());
-    inputs.wheelPositionRad = Units.rotationsToRadians(wheelMotor.getPosition().getValueAsDouble());
+    inputs.wheelPositionRad = Units.rotationsToRadians(driveMotor.getPosition().getValueAsDouble());
     inputs.pivotAppliedVolts = pivotMotor.getAppliedOutput();
-    inputs.wheelAppliedVolts = wheelMotor.getTorqueCurrent().getValueAsDouble();
+    inputs.wheelAppliedVolts = driveMotor.getTorqueCurrent().getValueAsDouble();
     inputs.pivotCurrentDrawAmps = pivotMotor.getOutputCurrent();
-    inputs.wheelCurrentDrawAmps = wheelMotor.getMotorVoltage().getValueAsDouble();
+    inputs.wheelCurrentDrawAmps = driveMotor.getMotorVoltage().getValueAsDouble();
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
index 33e84a4..f0cb1c3 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
@@ -31,6 +31,8 @@ public class Turret extends SubsystemBase {
   private boolean atGoal = false;
   private Debouncer atGoalDebouncer = new Debouncer(0.2, DebounceType.kFalling);
 
+  private boolean zeroed = false;
+
   /** Creates a new Turret. */
   public Turret(ShooterSide side, TurretIO io) {
     this.side = side;
diff --git a/src/main/java/frc/robot/util/Direction.java b/src/main/java/frc/robot/util/Direction.java
index d7e244c..8da2266 100644
--- a/src/main/java/frc/robot/util/Direction.java
+++ b/src/main/java/frc/robot/util/Direction.java
@@ -1,6 +1,7 @@
 package frc.robot.util;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 
 /** Enum representing common compass directions (e.g., North, Northeast, East). */
 public enum Direction {
@@ -52,11 +53,11 @@ public int getDx() {
       case EAST:
       case NORTHEAST:
       case SOUTHEAST:
-        return 1;
+        return -1;
       case WEST:
       case NORTHWEST:
       case SOUTHWEST:
-        return -1;
+        return 1;
       default:
         return 0;
     }
@@ -77,6 +78,24 @@ public int getDy() {
     }
   }
 
+  public ChassisSpeeds toChassisSpeeds() {
+    double dx = getDx();
+    double dy = getDy();
+
+    // Normalize so diagonals aren't faster
+    double magnitude = Math.hypot(dx, dy);
+    if (magnitude > 0) {
+      dx /= magnitude;
+      dy /= magnitude;
+    }
+
+    // Build chassis speeds (no rotation)
+    return new ChassisSpeeds(
+        dy, // vx = forward
+        dx, // vy = left
+        0.0);
+  }
+
   public boolean isCardinal() {
     return this == NORTH || this == SOUTH || this == EAST || this == WEST;
   }
@@ -86,7 +105,7 @@ public boolean isDiagonal() {
   }
 
   public static Direction fromAngle(double angleDegrees) {
-    angleDegrees = ((angleDegrees % 360) + 360) % 360; // normalize 0–359
+    angleDegrees = ((angleDegrees % 360) + 360) % 360; // Normalize 0–359
     int index = (int) Math.round(angleDegrees / 45.0) % 8;
     return values()[index];
   }
diff --git a/src/main/java/frc/robot/util/tuner-swerve-project.json b/src/main/java/frc/robot/util/tuner-swerve-project.json
new file mode 100644
index 0000000..1c73dac
--- /dev/null
+++ b/src/main/java/frc/robot/util/tuner-swerve-project.json
@@ -0,0 +1,232 @@
+{
+  "Version": "1.0.0.0",
+  "LastState": 11,
+  "Modules": [
+    {
+      "ModuleName": "Front Left",
+      "ModuleId": 0,
+      "Encoder": {
+        "Id": 19,
+        "Name": "FL CANCoder",
+        "Model": "CANCoder",
+        "CANbus": "rio",
+        "CANbusFriendly": "",
+        "SelectedMotorType": null
+      },
+      "SteerMotor": {
+        "Id": 2,
+        "Name": "FL Angle",
+        "Model": "Talon FX vers. F",
+        "CANbus": "rio",
+        "CANbusFriendly": "",
+        "SelectedMotorType": {
+          "Name": "WCP Kraken x44",
+          "FreeSpeedRps": 125.5,
+          "SlipCurrentLimit": 120,
+          "StatorCurrentLimit": 60
+        }
+      },
+      "DriveMotor": {
+        "Id": 1,
+        "Name": "FL Drive",
+        "Model": "Talon FX vers. C",
+        "CANbus": "rio",
+        "CANbusFriendly": "",
+        "SelectedMotorType": {
+          "Name": "WCP Kraken x60",
+          "FreeSpeedRps": 96.7,
+          "SlipCurrentLimit": 120,
+          "StatorCurrentLimit": 60
+        }
+      },
+      "IsEncoderInverted": false,
+      "IsSteerInverted": false,
+      "SelectedEncoderType": "CANcoder",
+      "EncoderOffset": -0.282470703125,
+      "DriveMotorSelectionState": 1,
+      "SteerMotorSelectionState": 1,
+      "SteerEncoderSelectionState": 1,
+      "IsModuleValidationComplete": true,
+      "ValidatedSteerId": 2,
+      "ValidatedDriveId": 1,
+      "ValidatedEncoderId": 19
+    },
+    {
+      "ModuleName": "Front Right",
+      "ModuleId": 1,
+      "Encoder": {
+        "Id": 20,
+        "Name": "FR CANCoder",
+        "Model": "CANCoder",
+        "CANbus": "rio",
+        "CANbusFriendly": "",
+        "SelectedMotorType": null
+      },
+      "SteerMotor": {
+        "Id": 4,
+        "Name": "FR Angle",
+        "Model": "Talon FX vers. F",
+        "CANbus": "rio",
+        "CANbusFriendly": "",
+        "SelectedMotorType": {
+          "Name": "WCP Kraken x44",
+          "FreeSpeedRps": 125.5,
+          "SlipCurrentLimit": 120,
+          "StatorCurrentLimit": 60
+        }
+      },
+      "DriveMotor": {
+        "Id": 3,
+        "Name": "FR Drive",
+        "Model": "Talon FX vers. C",
+        "CANbus": "rio",
+        "CANbusFriendly": "",
+        "SelectedMotorType": {
+          "Name": "WCP Kraken x60",
+          "FreeSpeedRps": 96.7,
+          "SlipCurrentLimit": 120,
+          "StatorCurrentLimit": 60
+        }
+      },
+      "IsEncoderInverted": false,
+      "IsSteerInverted": false,
+      "SelectedEncoderType": "CANcoder",
+      "EncoderOffset": -0.311767578125,
+      "DriveMotorSelectionState": 1,
+      "SteerMotorSelectionState": 1,
+      "SteerEncoderSelectionState": 1,
+      "IsModuleValidationComplete": true,
+      "ValidatedSteerId": 4,
+      "ValidatedDriveId": 3,
+      "ValidatedEncoderId": 20
+    },
+    {
+      "ModuleName": "Back Left",
+      "ModuleId": 2,
+      "Encoder": {
+        "Id": 21,
+        "Name": "BL CANCoder",
+        "Model": "CANCoder",
+        "CANbus": "rio",
+        "CANbusFriendly": "",
+        "SelectedMotorType": null
+      },
+      "SteerMotor": {
+        "Id": 6,
+        "Name": "BL Angle",
+        "Model": "Talon FX vers. F",
+        "CANbus": "rio",
+        "CANbusFriendly": "",
+        "SelectedMotorType": {
+          "Name": "WCP Kraken x44",
+          "FreeSpeedRps": 125.5,
+          "SlipCurrentLimit": 120,
+          "StatorCurrentLimit": 60
+        }
+      },
+      "DriveMotor": {
+        "Id": 5,
+        "Name": "BL Drive",
+        "Model": "Talon FX vers. C",
+        "CANbus": "rio",
+        "CANbusFriendly": "",
+        "SelectedMotorType": {
+          "Name": "WCP Kraken x60",
+          "FreeSpeedRps": 96.7,
+          "SlipCurrentLimit": 120,
+          "StatorCurrentLimit": 60
+        }
+      },
+      "IsEncoderInverted": false,
+      "IsSteerInverted": false,
+      "SelectedEncoderType": "CANcoder",
+      "EncoderOffset": -0.05712890625,
+      "DriveMotorSelectionState": 1,
+      "SteerMotorSelectionState": 1,
+      "SteerEncoderSelectionState": 1,
+      "IsModuleValidationComplete": true,
+      "ValidatedSteerId": 6,
+      "ValidatedDriveId": 5,
+      "ValidatedEncoderId": 21
+    },
+    {
+      "ModuleName": "Back Right",
+      "ModuleId": 3,
+      "Encoder": {
+        "Id": 22,
+        "Name": "BR CANCoder",
+        "Model": "CANCoder",
+        "CANbus": "rio",
+        "CANbusFriendly": "",
+        "SelectedMotorType": null
+      },
+      "SteerMotor": {
+        "Id": 8,
+        "Name": "BR Angle",
+        "Model": "Talon FX vers. F",
+        "CANbus": "rio",
+        "CANbusFriendly": "",
+        "SelectedMotorType": {
+          "Name": "WCP Kraken x44",
+          "FreeSpeedRps": 125.5,
+          "SlipCurrentLimit": 120,
+          "StatorCurrentLimit": 60
+        }
+      },
+      "DriveMotor": {
+        "Id": 7,
+        "Name": "BR Drive",
+        "Model": "Talon FX vers. C",
+        "CANbus": "rio",
+        "CANbusFriendly": "",
+        "SelectedMotorType": {
+          "Name": "WCP Kraken x60",
+          "FreeSpeedRps": 96.7,
+          "SlipCurrentLimit": 120,
+          "StatorCurrentLimit": 60
+        }
+      },
+      "IsEncoderInverted": false,
+      "IsSteerInverted": false,
+      "SelectedEncoderType": "CANcoder",
+      "EncoderOffset": 0.193603515625,
+      "DriveMotorSelectionState": 1,
+      "SteerMotorSelectionState": 1,
+      "SteerEncoderSelectionState": 1,
+      "IsModuleValidationComplete": true,
+      "ValidatedSteerId": 8,
+      "ValidatedDriveId": 7,
+      "ValidatedEncoderId": 22
+    }
+  ],
+  "SwerveOptions": {
+    "Gyro": {
+      "Id": 23,
+      "Name": "Pigeon 2 vers. S (Device ID 23)",
+      "Model": "Pigeon 2 vers. S",
+      "CANbus": "rio",
+      "CANbusFriendly": "",
+      "SelectedMotorType": null
+    },
+    "IsValidGyroCANbus": true,
+    "VerticalTrackSizeInches": 18.75,
+    "HorizontalTrackSizeInches": 25.75,
+    "WheelRadiusInches": 2.0,
+    "IsLeftSideInverted": false,
+    "IsRightSideInverted": true,
+    "SwerveModuleType": 7,
+    "SwerveModuleConfiguration": {
+      "ModuleBrand": 7,
+      "DriveRatio": 5.2734375,
+      "SteerRatio": 26.09090909090909,
+      "CouplingRatio": 3.375,
+      "CustomName": "R3"
+    },
+    "HasVerifiedSteer": true,
+    "SelectedModuleManufacturer": "Swerve Drive Specialties (SDS)",
+    "HasVerifiedDrive": true,
+    "IsValidConfiguration": true
+  },
+  "TeamNumber": 0,
+  "schema_version": "1"
+}
diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json
index 2faa4db..91b4e34 100644
--- a/vendordeps/AdvantageKit.json
+++ b/vendordeps/AdvantageKit.json
@@ -1,7 +1,7 @@
 {
   "fileName": "AdvantageKit.json",
   "name": "AdvantageKit",
-  "version": "26.0.0",
+  "version": "26.0.1",
   "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
   "frcYear": "2026",
   "mavenUrls": [
@@ -12,14 +12,14 @@
     {
       "groupId": "org.littletonrobotics.akit",
       "artifactId": "akit-java",
-      "version": "26.0.0"
+      "version": "26.0.1"
     }
   ],
   "jniDependencies": [
     {
       "groupId": "org.littletonrobotics.akit",
       "artifactId": "akit-wpilibio",
-      "version": "26.0.0",
+      "version": "26.0.1",
       "skipInvalidPlatforms": false,
       "isJar": false,
       "validPlatforms": [
diff --git a/vendordeps/Phoenix6-26.1.0.json b/vendordeps/Phoenix6-26.1.1.json
similarity index 92%
rename from vendordeps/Phoenix6-26.1.0.json
rename to vendordeps/Phoenix6-26.1.1.json
index 5d2d04d..c0a1c19 100644
--- a/vendordeps/Phoenix6-26.1.0.json
+++ b/vendordeps/Phoenix6-26.1.1.json
@@ -1,7 +1,7 @@
 {
-  "fileName": "Phoenix6-26.1.0.json",
+  "fileName": "Phoenix6-26.1.1.json",
   "name": "CTRE-Phoenix (v6)",
-  "version": "26.1.0",
+  "version": "26.1.1",
   "frcYear": "2026",
   "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
   "mavenUrls": [
@@ -19,14 +19,14 @@
     {
       "groupId": "com.ctre.phoenix6",
       "artifactId": "wpiapi-java",
-      "version": "26.1.0"
+      "version": "26.1.1"
     }
   ],
   "jniDependencies": [
     {
       "groupId": "com.ctre.phoenix6",
       "artifactId": "api-cpp",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -40,7 +40,7 @@
     {
       "groupId": "com.ctre.phoenix6",
       "artifactId": "tools",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -54,7 +54,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "api-cpp-sim",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -68,7 +68,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "tools-sim",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -82,7 +82,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simTalonSRX",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -96,7 +96,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simVictorSPX",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -110,7 +110,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simPigeonIMU",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -124,7 +124,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProTalonFX",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -138,7 +138,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProTalonFXS",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -152,7 +152,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProCANcoder",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -166,7 +166,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProPigeon2",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -180,7 +180,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProCANrange",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -194,7 +194,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProCANdi",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -208,7 +208,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProCANdle",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "isJar": false,
       "skipInvalidPlatforms": true,
       "validPlatforms": [
@@ -224,7 +224,7 @@
     {
       "groupId": "com.ctre.phoenix6",
       "artifactId": "wpiapi-cpp",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "libName": "CTRE_Phoenix6_WPI",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -240,7 +240,7 @@
     {
       "groupId": "com.ctre.phoenix6",
       "artifactId": "tools",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "libName": "CTRE_PhoenixTools",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -256,7 +256,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "wpiapi-cpp-sim",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "libName": "CTRE_Phoenix6_WPISim",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -272,7 +272,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "tools-sim",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "libName": "CTRE_PhoenixTools_Sim",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -288,7 +288,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simTalonSRX",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "libName": "CTRE_SimTalonSRX",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -304,7 +304,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simVictorSPX",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "libName": "CTRE_SimVictorSPX",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -320,7 +320,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simPigeonIMU",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "libName": "CTRE_SimPigeonIMU",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -336,7 +336,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProTalonFX",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "libName": "CTRE_SimProTalonFX",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -352,7 +352,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProTalonFXS",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "libName": "CTRE_SimProTalonFXS",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -368,7 +368,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProCANcoder",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "libName": "CTRE_SimProCANcoder",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -384,7 +384,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProPigeon2",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "libName": "CTRE_SimProPigeon2",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -400,7 +400,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProCANrange",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "libName": "CTRE_SimProCANrange",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -416,7 +416,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProCANdi",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "libName": "CTRE_SimProCANdi",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -432,7 +432,7 @@
     {
       "groupId": "com.ctre.phoenix6.sim",
       "artifactId": "simProCANdle",
-      "version": "26.1.0",
+      "version": "26.1.1",
       "libName": "CTRE_SimProCANdle",
       "headerClassifier": "headers",
       "sharedLibrary": true,
diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json
index bb613bf..1d80ce7 100644
--- a/vendordeps/REVLib.json
+++ b/vendordeps/REVLib.json
@@ -1,7 +1,7 @@
 {
   "fileName": "REVLib.json",
   "name": "REVLib",
-  "version": "2026.0.1",
+  "version": "2026.0.3",
   "frcYear": "2026",
   "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
   "mavenUrls": [
@@ -12,14 +12,14 @@
     {
       "groupId": "com.revrobotics.frc",
       "artifactId": "REVLib-java",
-      "version": "2026.0.1"
+      "version": "2026.0.3"
     }
   ],
   "jniDependencies": [
     {
       "groupId": "com.revrobotics.frc",
       "artifactId": "REVLib-driver",
-      "version": "2026.0.1",
+      "version": "2026.0.3",
       "skipInvalidPlatforms": true,
       "isJar": false,
       "validPlatforms": [
@@ -34,7 +34,7 @@
     {
       "groupId": "com.revrobotics.frc",
       "artifactId": "RevLibBackendDriver",
-      "version": "2026.0.1",
+      "version": "2026.0.3",
       "skipInvalidPlatforms": true,
       "isJar": false,
       "validPlatforms": [
@@ -49,7 +49,7 @@
     {
       "groupId": "com.revrobotics.frc",
       "artifactId": "RevLibWpiBackendDriver",
-      "version": "2026.0.1",
+      "version": "2026.0.3",
       "skipInvalidPlatforms": true,
       "isJar": false,
       "validPlatforms": [
@@ -66,7 +66,7 @@
     {
       "groupId": "com.revrobotics.frc",
       "artifactId": "REVLib-cpp",
-      "version": "2026.0.1",
+      "version": "2026.0.3",
       "libName": "REVLib",
       "headerClassifier": "headers",
       "sharedLibrary": false,
@@ -83,7 +83,7 @@
     {
       "groupId": "com.revrobotics.frc",
       "artifactId": "REVLib-driver",
-      "version": "2026.0.1",
+      "version": "2026.0.3",
       "libName": "REVLibDriver",
       "headerClassifier": "headers",
       "sharedLibrary": false,
@@ -100,7 +100,7 @@
     {
       "groupId": "com.revrobotics.frc",
       "artifactId": "RevLibBackendDriver",
-      "version": "2026.0.1",
+      "version": "2026.0.3",
       "libName": "BackendDriver",
       "sharedLibrary": true,
       "skipInvalidPlatforms": true,
@@ -116,7 +116,7 @@
     {
       "groupId": "com.revrobotics.frc",
       "artifactId": "RevLibWpiBackendDriver",
-      "version": "2026.0.1",
+      "version": "2026.0.3",
       "libName": "REVLibWpi",
       "sharedLibrary": true,
       "skipInvalidPlatforms": true,
diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json
index b0ac8fb..6279e58 100644
--- a/vendordeps/photonlib.json
+++ b/vendordeps/photonlib.json
@@ -1,7 +1,7 @@
 {
   "fileName": "photonlib.json",
   "name": "photonlib",
-  "version": "v2026.1.1",
+  "version": "v2026.2.2",
   "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
   "frcYear": "2026",
   "mavenUrls": [
@@ -13,7 +13,7 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photontargeting-cpp",
-      "version": "v2026.1.1",
+      "version": "v2026.2.2",
       "skipInvalidPlatforms": true,
       "isJar": false,
       "validPlatforms": [
@@ -28,7 +28,7 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photonlib-cpp",
-      "version": "v2026.1.1",
+      "version": "v2026.2.2",
       "libName": "photonlib",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -43,7 +43,7 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photontargeting-cpp",
-      "version": "v2026.1.1",
+      "version": "v2026.2.2",
       "libName": "photontargeting",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -60,12 +60,12 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photonlib-java",
-      "version": "v2026.1.1"
+      "version": "v2026.2.2"
     },
     {
       "groupId": "org.photonvision",
       "artifactId": "photontargeting-java",
-      "version": "v2026.1.1"
+      "version": "v2026.2.2"
     }
   ]
 }

From 043db118521f77414bdad634793c820b271610e3 Mon Sep 17 00:00:00 2001
From: Maxwell Morgan 
Date: Wed, 4 Mar 2026 11:32:48 -0500
Subject: [PATCH 42/61] Update turret values; add flywheel closed-loop config

---
 src/main/java/frc/robot/Robot.java            |   4 +-
 src/main/java/frc/robot/RobotContainer.java   |   5 +-
 .../frc/robot/control/DefaultControls.java    |   2 +-
 .../frc/robot/subsystems/shooter/Shooter.java |   6 +-
 .../subsystems/shooter/ShooterConstants.java  | 140 +++++++++---------
 .../shooter/flywheel/FlywheelIOTalonFX.java   |  37 +++--
 .../shooter/hood/HoodIOSparkMax.java          |  11 +-
 .../subsystems/shooter/turret/Turret.java     |  49 ++++--
 .../subsystems/shooter/turret/TurretIO.java   |  19 ++-
 .../shooter/turret/TurretIOSim.java           |  24 ++-
 .../shooter/turret/TurretIOSparkMax.java      |  45 +++---
 .../java/frc/robot/util/FullSubsystem.java    |  42 ++++++
 12 files changed, 246 insertions(+), 138 deletions(-)
 create mode 100644 src/main/java/frc/robot/util/FullSubsystem.java

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 1fb2f38..1e41966 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.util.CachedSupplier;
+import frc.robot.util.FullSubsystem;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
@@ -74,9 +75,10 @@ public Robot() {
   /** This function is called periodically during all modes. */
   @Override
   public void robotPeriodic() {
-    CachedSupplier.invalidateAll();
     robotContainer.robotPeriodic();
     CommandScheduler.getInstance().run();
+    FullSubsystem.runAllPeriodicAfterScheduler();
+    CachedSupplier.invalidateAll();
 
     RobotVisualizer.getInstance().log("Mechanism3d/Robot");
   }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 03af959..3d6f398 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -12,9 +12,9 @@
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.RobotState.OdometryObservation;
+import frc.robot.commands.DriveCommands;
 import frc.robot.control.Configurable;
 import frc.robot.control.DefaultControls;
 import frc.robot.control.DriverController;
@@ -156,7 +156,8 @@ public void robotPeriodic() {
   }
 
   public Command getAutonomousCommand() {
-    return Commands.print("No autonomous command configured");
+    return leftShooter.zeroTurret().alongWith(rightShooter.zeroTurret());
+    // return Commands.print("No autonomous command configured");
   }
 
   public void configurePathPlanner() {
diff --git a/src/main/java/frc/robot/control/DefaultControls.java b/src/main/java/frc/robot/control/DefaultControls.java
index 07b7103..66ff50d 100644
--- a/src/main/java/frc/robot/control/DefaultControls.java
+++ b/src/main/java/frc/robot/control/DefaultControls.java
@@ -32,6 +32,6 @@ public DefaultControls(
   public void configure() {
     drive.setDefaultCommand(
         DriveCommands.joystickDrive(
-            drive, () -> driver.getLeftY(), () -> driver.getLeftX(), () -> -driver.getRightX()));
+            drive, () -> -driver.getLeftY(), () -> -driver.getLeftX(), () -> -driver.getRightX()));
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index ff159eb..db5386a 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -96,7 +96,11 @@ public Command shootAtTarget(Supplier targetSupplier) {
   }
 
   public Command trackTarget(Supplier targetSupplier) {
-    return Commands.idle(this).alongWith(turret.trackTarget(targetSupplier));
+    return turret.trackTarget(targetSupplier);
+  }
+
+  public Command zeroTurret() {
+    return turret.zero();
   }
 
   public void setFlywheelVelocity(double velocityRPM) {
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index 7d82b93..07ae0aa 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -13,76 +13,72 @@
 import frc.robot.util.GeomUtil;
 
 public final class ShooterConstants {
-  public static final double kLatencySeconds = 0.05;
-
-  public static final class TurretConstants {
-    public static final double kGearRatio = 10 / 1;
-    public static final double kMinTurretAngleRad = Units.degreesToRadians(-90);
-    public static final double kMaxTurretAngleRad = Units.degreesToRadians(90);
-    public static final double kAngleTolerance = Units.degreesToRadians(2);
-
-    public static final double kLeftMotorId = 12;
-    public static final double kRightMotorId = 13;
-
-    // +X = Forward, +Y = Left
-    public static final Transform3d kRobotToLeftTurret =
-        new Transform3d(Inches.of(3.749), Inches.of(8.186), Inches.of(13.401), Rotation3d.kZero);
-
-    public static final Transform3d kRobotToRightTurret =
-        new Transform3d(Inches.of(3.749), Inches.of(-8.314), Inches.of(13.401), Rotation3d.kZero);
-  }
-
-  public static final class HoodConstants {
-    public static final double kTurretToHoodInches = 1.878;
-    public static final double kGearRatio = 100 / 1;
-
-    public static final double kLeftHoodID = -1;
-    public static final double kRightHoodID = -1;
-
-    public static final double kAngleTolerance = Units.degreesToRadians(5);
-
-    public static final Transform3d kRobotToLeftHood =
-        new Transform3d(
-            Inches.of(7.268715), Meters.of(0.20792316), Inches.of(16.018516), Rotation3d.kZero);
-
-    public static final Transform3d kRobotToRightHood =
-        new Transform3d(
-            Inches.of(-7.270121),
-            Inches.of(-(12.062888 - (7.5 / 2.0))),
-            Inches.of(16.018516),
-            Rotation3d.kZero);
-
-    public static final Transform3d kLeftTurretToLeftHood =
-        GeomUtil.toPose3d(HoodConstants.kRobotToLeftHood)
-            .minus(
-                GeomUtil.toPose3d(TurretConstants.kRobotToLeftTurret)
-                    .plus(
-                        new Transform3d(
-                            Inches.of(7.268715), Inches.of(0), Inches.of(0), new Rotation3d())));
-
-    public static final Transform3d kRightTurretToRightHood =
-        GeomUtil.toPose3d(HoodConstants.kRobotToRightHood)
-            .minus(
-                GeomUtil.toPose3d(TurretConstants.kRobotToRightTurret)
-                    .plus(
-                        new Transform3d(
-                            Inches.of(-7.270121), Inches.of(0), Inches.of(0), new Rotation3d())));
-
-    public static final double kMinAngleRad = Units.degreesToRadians(0);
-    public static final double kMaxAngleRad = Units.degreesToRadians(30);
-  }
-
-  public static final class FlywheelConstants {
-    public static final double kGearRatio = 300;
-    public static final double kSpeedTolerance = 25.0;
-
-    public static final int kLeftFlywheelID = -1;
-    public static final int kRightFlywheelID = -1;
-
-    public static final Slot0Configs kGains = new Slot0Configs().withKP(1).withKD(0).withKS(0);
-    public static final MotorOutputConfigs kOutputConfigs =
-        new MotorOutputConfigs()
-            .withNeutralMode(NeutralModeValue.Coast)
-            .withInverted(InvertedValue.Clockwise_Positive);
-  }
+    public static final double kLatencySeconds = 0.05;
+
+    public static final class TurretConstants {
+        public static final double kGearRatio = 10 / 1;
+        public static final double kMinTurretAngleRad = Units.degreesToRadians(-90);
+        public static final double kMaxTurretAngleRad = Units.degreesToRadians(90);
+        public static final double kAngleTolerance = Units.degreesToRadians(2);
+
+        public static final double kLeftMotorId = 12;
+        public static final double kRightMotorId = 13;
+
+        // +X = Forward, +Y = Left
+        public static final Transform3d kRobotToLeftTurret = new Transform3d(Inches.of(3.749), Inches.of(8.186),
+                Inches.of(13.401), Rotation3d.kZero);
+
+        public static final Transform3d kRobotToRightTurret = new Transform3d(Inches.of(3.749), Inches.of(-8.314),
+                Inches.of(13.401), Rotation3d.kZero);
+    }
+
+    public static final class HoodConstants {
+        public static final double kTurretToHoodInches = 1.878;
+        public static final double kGearRatio = 100 / 1;
+
+        public static final double kLeftHoodID = -1;
+        public static final double kRightHoodID = -1;
+
+        public static final double kAngleTolerance = Units.degreesToRadians(5);
+
+        public static final Transform3d kRobotToLeftHood = new Transform3d(
+                Inches.of(7.268715), Meters.of(0.20792316), Inches.of(16.018516), Rotation3d.kZero);
+
+        public static final Transform3d kRobotToRightHood = new Transform3d(
+                Inches.of(-7.270121),
+                Inches.of(-(12.062888 - (7.5 / 2.0))),
+                Inches.of(16.018516),
+                Rotation3d.kZero);
+
+        public static final Transform3d kLeftTurretToLeftHood = GeomUtil.toPose3d(HoodConstants.kRobotToLeftHood)
+                .minus(
+                        GeomUtil.toPose3d(TurretConstants.kRobotToLeftTurret)
+                                .plus(
+                                        new Transform3d(
+                                                Inches.of(7.268715), Inches.of(0), Inches.of(0), new Rotation3d())));
+
+        public static final Transform3d kRightTurretToRightHood = GeomUtil.toPose3d(HoodConstants.kRobotToRightHood)
+                .minus(
+                        GeomUtil.toPose3d(TurretConstants.kRobotToRightTurret)
+                                .plus(
+                                        new Transform3d(
+                                                Inches.of(-7.270121), Inches.of(0), Inches.of(0), new Rotation3d())));
+
+        public static final double kMinAngleRad = Units.degreesToRadians(0);
+        public static final double kMaxAngleRad = Units.degreesToRadians(30);
+    }
+
+    public static final class FlywheelConstants {
+        public static final double kGearRatio = 300;
+        public static final double kSpeedTolerance = 25.0;
+
+        public static final int kLeftFlywheelID = -1;
+        public static final int kRightFlywheelID = -1;
+
+        public static final Slot0Configs kGains = new Slot0Configs().withKP(0).withKI(0).withKD(0).withKS(0).withKV(0)
+                .withKA(0);
+        public static final MotorOutputConfigs kOutputConfigs = new MotorOutputConfigs()
+                .withNeutralMode(NeutralModeValue.Coast)
+                .withInverted(InvertedValue.Clockwise_Positive);
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java
index a7299a2..ee689ce 100644
--- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java
@@ -5,13 +5,17 @@
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
 import edu.wpi.first.units.measure.AngularAcceleration;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.units.measure.Voltage;
+import frc.robot.Constants.DeviceIDs;
+import frc.robot.subsystems.shooter.Shooter.ShooterSide;
 import frc.robot.subsystems.shooter.ShooterConstants.FlywheelConstants;
 
 public class FlywheelIOTalonFX implements FlywheelIO {
@@ -25,12 +29,26 @@ public class FlywheelIOTalonFX implements FlywheelIO {
 
   private final VelocityVoltage velocityRequest = new VelocityVoltage(0).withSlot(0);
 
-  public FlywheelIOTalonFX(int motorID) {
-    motor = new TalonFX(motorID);
-    motorConfig =
-        new TalonFXConfiguration()
-            .withSlot0(FlywheelConstants.kGains)
-            .withMotorOutput(FlywheelConstants.kOutputConfigs);
+  public FlywheelIOTalonFX(ShooterSide side) {
+    motor = new TalonFX(
+        side == ShooterSide.LEFT
+            ? DeviceIDs.kLeftTurretFlywheel
+            : DeviceIDs.kRightTurretFlywheel);
+    motorConfig = new TalonFXConfiguration()
+        .withMotorOutput(
+            new MotorOutputConfigs()
+                .withInverted(
+                    side == ShooterSide.LEFT
+                        ? InvertedValue.Clockwise_Positive
+                        : InvertedValue.CounterClockwise_Positive))
+        .withSlot0(FlywheelConstants.kGains)
+        /**
+         * TODO: Update gains
+         * Peiwei, Ben: see the FlywheelConstants.kGains above... thats where the values are
+         * You also might have to check if the inverted values are correct, positive
+         * should spin the right way for shooting (line above that has the withInverted() method)
+         */
+        .withMotorOutput(FlywheelConstants.kOutputConfigs);
     tryUntilOk(5, () -> motor.getConfigurator().apply(motorConfig, 0.25));
 
     velocitySignal = motor.getVelocity();
@@ -45,10 +63,9 @@ public FlywheelIOTalonFX(int motorID) {
 
   @Override
   public void updateInputs(FlywheelIOInputs inputs) {
-    inputs.connected =
-        BaseStatusSignal.refreshAll(
-                velocitySignal, accelerationSignal, voltageSignal, currentSignal)
-            .isOK();
+    inputs.connected = BaseStatusSignal.refreshAll(
+        velocitySignal, accelerationSignal, voltageSignal, currentSignal)
+        .isOK();
     inputs.velocityRadPerSec = velocitySignal.getValue().in(RadiansPerSecond);
     inputs.appliedVolts = voltageSignal.getValueAsDouble();
     inputs.currentDrawAmps = currentSignal.getValueAsDouble();
diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
index 6e10146..943cd31 100644
--- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
@@ -13,6 +13,8 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
+import frc.robot.Constants.DeviceIDs;
+import frc.robot.subsystems.shooter.Shooter.ShooterSide;
 import frc.robot.subsystems.shooter.ShooterConstants.HoodConstants;
 import java.util.function.DoubleSupplier;
 
@@ -22,8 +24,11 @@ public class HoodIOSparkMax implements HoodIO {
   private final SparkClosedLoopController motorController;
   private final Debouncer connectedDebouncer = new Debouncer(0.5, DebounceType.kFalling);
 
-  public HoodIOSparkMax(int motorID) {
-    motor = new SparkMax(motorID, MotorType.kBrushless);
+  public HoodIOSparkMax(ShooterSide side) {
+    motor =
+        new SparkMax(
+            side == ShooterSide.LEFT ? DeviceIDs.kLeftTurretHood : DeviceIDs.kRightTurretHood,
+            MotorType.kBrushless);
     encoder = motor.getEncoder();
     motorController = motor.getClosedLoopController();
 
@@ -31,6 +36,8 @@ public HoodIOSparkMax(int motorID) {
 
     config.idleMode(IdleMode.kCoast);
 
+    config.inverted(side == ShooterSide.LEFT);
+
     config
         .encoder
         .positionConversionFactor(2 * Math.PI / HoodConstants.kGearRatio) // No absolute encoder...
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
index f0cb1c3..1bbf6aa 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
@@ -12,26 +12,29 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.RobotState;
 import frc.robot.RobotVisualizer;
 import frc.robot.subsystems.shooter.Shooter.ShooterSide;
 import frc.robot.subsystems.shooter.ShooterConstants.TurretConstants;
+import frc.robot.subsystems.shooter.turret.TurretIO.TurretIOOutputMode;
+import frc.robot.subsystems.shooter.turret.TurretIO.TurretIOOutputs;
+import frc.robot.util.FullSubsystem;
 import java.util.function.Supplier;
 import org.littletonrobotics.junction.Logger;
 
-public class Turret extends SubsystemBase {
+public class Turret extends FullSubsystem {
   private final ShooterSide side;
 
   private final TurretIO io;
   private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged();
+  private final TurretIOOutputs outputs = new TurretIOOutputs();
 
   private Rotation2d targetAngle = Rotation2d.kZero;
 
   private boolean atGoal = false;
-  private Debouncer atGoalDebouncer = new Debouncer(0.2, DebounceType.kFalling);
+  private Debouncer atGoalDebouncer = new Debouncer(0.1, DebounceType.kFalling);
 
-  private boolean zeroed = false;
+  private boolean isZeroed = false;
 
   /** Creates a new Turret. */
   public Turret(ShooterSide side, TurretIO io) {
@@ -44,6 +47,10 @@ public void periodic() {
     io.updateInputs(inputs);
     Logger.processInputs(("Turret/" + side.getName()), inputs);
 
+    if (inputs.limitTriggered) {
+      isZeroed = true;
+    }
+
     if (side == ShooterSide.LEFT) {
       RobotVisualizer.getInstance().setLeftTurretAngle(Rotation2d.fromRadians(inputs.positionRad));
     } else if (side == ShooterSide.RIGHT) {
@@ -53,6 +60,11 @@ public void periodic() {
     Logger.recordOutput(("Turret/" + side.getName() + "/TargetAngle"), targetAngle);
   }
 
+  @Override
+  public void periodicAfterScheduler() {
+    io.applyOutputs(outputs);
+  }
+
   public Command trackTarget(Supplier targetSupplier) {
 
     return Commands.run(
@@ -85,6 +97,10 @@ public Command trackTarget(Supplier targetSupplier) {
         this);
   }
 
+  public Command zero() {
+    return Commands.startEnd(() -> setOpenLoop(0.2), () -> stop()).until(this::isZeroed);
+  }
+
   /**
    * Set the target angle for the turret.
    *
@@ -93,9 +109,9 @@ public Command trackTarget(Supplier targetSupplier) {
    * @param position A {@link Rotation2d} object representing the target position of the turret.
    */
   public void setPosition(Rotation2d position) {
-    atGoal =
-        atGoalDebouncer.calculate(
-            Math.abs(position.getRadians() - inputs.positionRad) < TurretConstants.kAngleTolerance);
+    if (!isZeroed) return; // safety
+
+    targetAngle = position;
 
     position =
         Rotation2d.fromRadians(
@@ -104,11 +120,22 @@ public void setPosition(Rotation2d position) {
                 TurretConstants.kMinTurretAngleRad,
                 TurretConstants.kMaxTurretAngleRad));
 
-    io.setPosition(position);
+    outputs.mode = TurretIOOutputMode.CLOSED_LOOP;
+    outputs.closedLoopTarget = position;
+
+    atGoal =
+        atGoalDebouncer.calculate(
+            Math.abs(position.getRadians() - inputs.positionRad) < TurretConstants.kAngleTolerance);
   }
 
   public void setOpenLoop(double output) {
-    io.setOpenLoop(output);
+    outputs.mode = TurretIOOutputMode.OPEN_LOOP;
+    outputs.openLoopOutput = MathUtil.clamp(output, -1.0, 1.0);
+  }
+
+  public void stop() {
+    outputs.mode = TurretIOOutputMode.OPEN_LOOP;
+    outputs.openLoopOutput = 0.0;
   }
 
   public double getPosition() {
@@ -123,6 +150,10 @@ public boolean atGoal() {
     return atGoal;
   }
 
+  public boolean isZeroed() {
+    return isZeroed;
+  }
+
   public ShooterSide getSide() {
     return this.side;
   }
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java
index a3ad4d2..4efbe39 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java
@@ -4,7 +4,6 @@
 import org.littletonrobotics.junction.AutoLog;
 
 public interface TurretIO {
-  public default void updateInputs(TurretIOInputs inputs) {}
 
   @AutoLog
   public static class TurretIOInputs {
@@ -13,12 +12,22 @@ public static class TurretIOInputs {
     public double velocityRadPerSec = 0.0;
     public double appliedVolts = 0.0;
     public double currentDrawAmps = 0.0;
+    public boolean limitTriggered = false;
   }
 
-  public default void setPosition(Rotation2d position) {}
+  public static enum TurretIOOutputMode {
+    CLOSED_LOOP,
+    OPEN_LOOP
+  }
+
+  public class TurretIOOutputs {
+    public TurretIOOutputMode mode = TurretIOOutputMode.CLOSED_LOOP;
+
+    public double openLoopOutput = 0.0;
+    public Rotation2d closedLoopTarget = Rotation2d.kZero;
+  }
 
-  /** Run motor at the specified open loop value. */
-  public default void setOpenLoop(double output) {}
+  void updateInputs(TurretIOInputs inputs);
 
-  default void stop() {}
+  void applyOutputs(TurretIOOutputs outputs);
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSim.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSim.java
index 668be0e..2618e84 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSim.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSim.java
@@ -2,7 +2,6 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
@@ -41,18 +40,15 @@ public void updateInputs(TurretIOInputs inputs) {
   }
 
   @Override
-  public void setPosition(Rotation2d position) {
-    pid.setSetpoint(position.getRadians());
-    appliedVolts = pid.calculate(sim.getAngularPositionRad());
-  }
-
-  @Override
-  public void setOpenLoop(double output) {
-    appliedVolts = 12.0 * output;
-  }
-
-  @Override
-  public void stop() {
-    appliedVolts = 0.0;
+  public void applyOutputs(TurretIOOutputs outputs) {
+    switch (outputs.mode) {
+      case CLOSED_LOOP -> {
+        pid.setSetpoint(outputs.closedLoopTarget.getRadians());
+        appliedVolts = pid.calculate(sim.getAngularPositionRad());
+      }
+      case OPEN_LOOP -> {
+        appliedVolts = 12.0 * outputs.openLoopOutput;
+      }
+    }
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
index 7d1f133..79528ec 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
@@ -17,7 +17,8 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
-import edu.wpi.first.math.geometry.Rotation2d;
+import frc.robot.Constants.DeviceIDs;
+import frc.robot.subsystems.shooter.Shooter.ShooterSide;
 import frc.robot.subsystems.shooter.ShooterConstants.TurretConstants;
 import java.util.function.DoubleSupplier;
 
@@ -28,14 +29,19 @@ public class TurretIOSparkMax implements TurretIO {
 
   private final Debouncer connectedDebouncer = new Debouncer(0.5, DebounceType.kFalling);
 
-  public TurretIOSparkMax(int motorID) {
-    motor = new SparkMax(motorID, MotorType.kBrushless);
+  public TurretIOSparkMax(ShooterSide side) {
+    motor =
+        new SparkMax(
+            side == ShooterSide.LEFT ? DeviceIDs.kLeftTurretAzimuth : DeviceIDs.kRightTurretAzimuth,
+            MotorType.kBrushless);
     encoder = motor.getEncoder();
     motorController = motor.getClosedLoopController();
 
     SparkMaxConfig config = new SparkMaxConfig();
 
     config.idleMode(IdleMode.kCoast);
+    // TODO: Tune
+    config.inverted(side == ShooterSide.LEFT);
     // .smartCurrentLimit(30);
 
     config
@@ -78,23 +84,20 @@ public void updateInputs(TurretIOInputs inputs) {
   }
 
   @Override
-  public void setPosition(Rotation2d position) {
-    double clampedPosition =
-        MathUtil.clamp(
-            position.getRadians(),
-            TurretConstants.kMinTurretAngleRad,
-            TurretConstants.kMaxTurretAngleRad);
-
-    motorController.setSetpoint(clampedPosition, ControlType.kPosition);
-  }
-
-  @Override
-  public void setOpenLoop(double output) {
-    motor.set(MathUtil.clamp(output, -1.0, 1.0));
-  }
-
-  @Override
-  public void stop() {
-    motor.stopMotor();
+  public void applyOutputs(TurretIOOutputs outputs) {
+    switch (outputs.mode) {
+      case CLOSED_LOOP -> {
+        double clampedPosition =
+            MathUtil.clamp(
+                outputs.closedLoopTarget.getRadians(),
+                TurretConstants.kMinTurretAngleRad,
+                TurretConstants.kMaxTurretAngleRad);
+
+        motorController.setSetpoint(clampedPosition, ControlType.kPosition);
+      }
+      case OPEN_LOOP -> {
+        motor.set(MathUtil.clamp(outputs.openLoopOutput, -1.0, 1.0));
+      }
+    }
   }
 }
diff --git a/src/main/java/frc/robot/util/FullSubsystem.java b/src/main/java/frc/robot/util/FullSubsystem.java
new file mode 100644
index 0000000..5fc74db
--- /dev/null
+++ b/src/main/java/frc/robot/util/FullSubsystem.java
@@ -0,0 +1,42 @@
+// Copyright (c) 2025-2026 Littleton Robotics
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+package frc.robot.util;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * A standard subsystem that includes an extra periodic callback which runs after the command
+ * scheduler. Allows outputs to be published after all other periodic code has finished.
+ */
+public abstract class FullSubsystem extends SubsystemBase {
+  private static List instances = new ArrayList<>();
+
+  public FullSubsystem() {
+    super();
+    instances.add(this);
+  }
+
+  public FullSubsystem(String name) {
+    super(name);
+    instances.add(this);
+  }
+
+  /**
+   * This method is called periodically after the command scheduler, and should be used for applying
+   * outputs.
+   */
+  public abstract void periodicAfterScheduler();
+
+  /** Run the "after periodic" methods for all subsystems. */
+  public static void runAllPeriodicAfterScheduler() {
+    for (FullSubsystem instance : instances) {
+      instance.periodicAfterScheduler();
+    }
+  }
+}

From 6779d88c36cb7f4c8aac8465e89f07e41dff43cf Mon Sep 17 00:00:00 2001
From: Maxwell Morgan 
Date: Wed, 4 Mar 2026 17:33:29 -0500
Subject: [PATCH 43/61] Remove phoenix pro feature

---
 .../frc/robot/control/DriverControls.java     | 36 +++++++++----------
 .../subsystems/drive/DriveConstants.java      |  2 +-
 .../frc/robot/subsystems/intake/Intake.java   |  2 +-
 .../frc/robot/subsystems/intake/IntakeIO.java |  5 +++
 .../subsystems/intake/IntakeIOHardware.java   |  6 ++++
 .../subsystems/shooter/ShooterConstants.java  |  2 +-
 .../shooter/hood/HoodIOSparkMax.java          | 11 ++++++
 7 files changed, 42 insertions(+), 22 deletions(-)

diff --git a/src/main/java/frc/robot/control/DriverControls.java b/src/main/java/frc/robot/control/DriverControls.java
index cf7aa17..e08b2b9 100644
--- a/src/main/java/frc/robot/control/DriverControls.java
+++ b/src/main/java/frc/robot/control/DriverControls.java
@@ -54,14 +54,6 @@ public void configure() {
     // Neutral controls (regardless of whether we are in one or two driver mode)
     driver.xSquare().onTrue(Commands.runOnce(drive::zeroYaw));
 
-    driver
-        .bCircle()
-        .onTrue(
-            Commands.runEnd(
-                    () -> driver.rumble(RumbleType.kBothRumble, 1),
-                    () -> driver.rumble(RumbleType.kBothRumble, 0.0))
-                .withTimeout(0.25));
-
     driver.dPadUp().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTH));
     driver.dPadUpLeft().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTHWEST));
     driver.dPadUpRight().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTHEAST));
@@ -78,26 +70,31 @@ public void configure() {
   /*
    * Driver Bindings:
    *
-   * 
LB: Toggle deploy/retract intake LT: Spin intake RB: Shoot LT + A:
-   * backspin intake RT:
-   * Climb RT + A: Unclimb X: reset Gyro D-Pad: CrabWalk LB + RB + Y: Aux Handoff
+   * LB: Toggle deploy/retract intake
+   * LT: Spin intake
+   * RB: Shoot
+   * LT + A: backspin intake
+   * RT: Climb RT + A: Unclimb
+   * X: reset Gyro
+   * D-Pad: CrabWalk
+   * LB + RB + Y: Aux Handoff
    *
    */
-
   private void configureOneDriver() {
 
     // driver
-    //     .rightBumper()
-    //     .and(this::isOneDriver)
-    //     .onTrue(Shooter.shootBothAtHub(leftShooter, rightShooter));
+    // .rightBumper()
+    // .and(this::isOneDriver)
+    // .onTrue(Shooter.shootBothAtHub(leftShooter, rightShooter));
 
     // driver.leftBumper().and(this::isOneDriver).onTrue(intake.deploy().withTimeout(0.5));
 
-    // driver.leftBumper().and(this::isOneDriver).onTrue(intake.retract().withTimeout(0.5));
+    driver.leftBumper().and(this::isOneDriver).onTrue(intake.retract().withTimeout(0.5));
 
-    // driver.leftTrigger().and(this::isOneDriver).whileTrue(intake.intake());
+    driver.leftTrigger().and(this::isOneDriver).whileTrue(intake.intake());
 
-    // driver.leftTrigger().and(this::isOneDriver).whileTrue(intake.outtake());
+    driver.leftBumper().and(driver.rightBumper()).and(driver.yTriangle())
+        .onTrue(Commands.runOnce(() -> this.setMode(DriverMode.TWO_DRIVERS)));
   }
 
   /*
@@ -113,7 +110,8 @@ private void configureOneDriver() {
    *
    * 
thumb button: Driver Handoff
    */
-  private void configureTwoDrivers() {}
+  private void configureTwoDrivers() {
+  }
 
   private boolean isOneDriver() {
     return mode == DriverMode.ONE_DRIVER;
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
index 54a22f2..d532999 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -116,7 +116,7 @@ public class TunerConstants {
 
     // The remote sensor feedback type to use for the steer motors;
     // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote*
-    private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
+    private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.RemoteCANcoder;
 
     // The stator current at which the wheels start to slip;
     // This needs to be tuned to your individual robot
diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
index e09517f..d227d65 100644
--- a/src/main/java/frc/robot/subsystems/intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -62,7 +62,7 @@ public Command intake() {
    */
   public Command outtake() {
     return Commands.runEnd(
-        () -> io.setWheelSpeed(-(IntakeConstants.kRollerMotorSpeed)),
+        () -> io.setWheelSpeed(-IntakeConstants.kRollerMotorSpeed),
         () -> io.setWheelSpeed(0.0),
         this);
   }
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
index 448a479..f8f2b86 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
@@ -18,14 +18,19 @@ default void updateInputs(IntakeIOInputs inputs) {}
   public static class IntakeIOInputs {
     public double pivotVelocityRadPerSec = 0.0;
     public double wheelVelocityRadPerSec = 0.0;
+
     public double pivotPositionRad = 0.0;
     public double wheelPositionRad = 0.0;
+
     public double pivotAppliedVolts = 0.0;
     public double wheelAppliedVolts = 0.0;
+    
     public double pivotCurrentDrawAmps = 0.0;
     public double wheelCurrentDrawAmps = 0.0;
   }
 
+  default void setPivotPosition(double positionRad) {}
+
   /**
    * method to set the speed of the pivot
    *
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java
index 1f33515..bcdb012 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java
@@ -25,6 +25,12 @@ public IntakeIOHardware() {
         pivotConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
   }
 
+  @Override
+  public void setPivotPosition(double positionRad) {
+      // TODO Auto-generated method stub
+      IntakeIO.super.setPivotPosition(positionRad);
+  }
+
   @Override
   public void setPivotSpeed(double speed) {
     pivotMotor.set(speed);
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index 07ae0aa..6fa24bd 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -16,7 +16,7 @@ public final class ShooterConstants {
     public static final double kLatencySeconds = 0.05;
 
     public static final class TurretConstants {
-        public static final double kGearRatio = 10 / 1;
+        public static final double kGearRatio = 10 / 1; // Motor / Turret
         public static final double kMinTurretAngleRad = Units.degreesToRadians(-90);
         public static final double kMaxTurretAngleRad = Units.degreesToRadians(90);
         public static final double kAngleTolerance = Units.degreesToRadians(2);
diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
index 943cd31..e018154 100644
--- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
@@ -1,9 +1,12 @@
 package frc.robot.subsystems.shooter.hood;
 
+import static frc.robot.util.SparkUtil.tryUntilOk;
 import static frc.robot.util.SparkUtil.ifOk;
 import static frc.robot.util.SparkUtil.sparkStickyFault;
 
+import com.revrobotics.PersistMode;
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.ResetMode;
 import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
@@ -44,6 +47,14 @@ public HoodIOSparkMax(ShooterSide side) {
         .velocityConversionFactor(2 * Math.PI / HoodConstants.kGearRatio / 60.0);
 
     config.closedLoop.feedForward.kS(0);
+
+    tryUntilOk(
+        motor,
+        5,
+        () ->
+            motor.configure(
+                config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters));
+    tryUntilOk(motor, 5, () -> encoder.setPosition(0));
   }
 
   @Override

From 41ab4e7eed4ba88f3dd87d8f145be0fee09aec0d Mon Sep 17 00:00:00 2001
From: Maxwell Morgan 
Date: Thu, 5 Mar 2026 07:28:48 -0500
Subject: [PATCH 44/61] Update controls

---
 src/main/java/frc/robot/RobotContainer.java   |  54 +++----
 .../frc/robot/control/DriverControls.java     |  39 +++--
 .../subsystems/drive/DriveConstants.java      |   8 +-
 .../robot/subsystems/drive/GyroIOPigeon2.java |   2 +-
 .../java/frc/robot/subsystems/guts/Guts.java  |   3 +-
 .../robot/subsystems/guts/GutsConstants.java  |   2 +-
 .../subsystems/intake/IntakeConstants.java    |   2 +-
 .../frc/robot/subsystems/intake/IntakeIO.java |   2 +-
 .../subsystems/intake/IntakeIOHardware.java   |   7 +-
 .../robot/subsystems/leds/LedConstants.java   |  25 ++++
 .../java/frc/robot/subsystems/leds/Leds.java  | 137 +++++++++++++++++
 .../subsystems/shooter/ShooterConstants.java  | 141 +++++++++---------
 .../shooter/flywheel/FlywheelIOTalonFX.java   |  43 +++---
 .../shooter/hood/HoodIOSparkMax.java          |   2 +-
 14 files changed, 323 insertions(+), 144 deletions(-)
 create mode 100644 src/main/java/frc/robot/subsystems/leds/LedConstants.java
 create mode 100644 src/main/java/frc/robot/subsystems/leds/Leds.java

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 3d6f398..5112164 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -11,10 +11,11 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.Constants.DeviceIDs;
 import frc.robot.RobotState.OdometryObservation;
-import frc.robot.commands.DriveCommands;
 import frc.robot.control.Configurable;
 import frc.robot.control.DefaultControls;
 import frc.robot.control.DriverController;
@@ -29,13 +30,21 @@
 import frc.robot.subsystems.guts.Guts;
 import frc.robot.subsystems.guts.Guts.GutSide;
 import frc.robot.subsystems.guts.GutsIOSim;
+import frc.robot.subsystems.guts.GutsIOSparkMax;
 import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.intake.IntakeIOHardware;
 import frc.robot.subsystems.intake.IntakeIOSim;
+import frc.robot.subsystems.leds.Leds;
+import frc.robot.subsystems.leds.Leds.LedSection;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.Shooter.ShooterSide;
 import frc.robot.subsystems.shooter.flywheel.FlywheelIOSim;
+import frc.robot.subsystems.shooter.flywheel.FlywheelIOTalonFX;
 import frc.robot.subsystems.shooter.hood.HoodIOSim;
+import frc.robot.subsystems.shooter.hood.HoodIOSparkMax;
 import frc.robot.subsystems.shooter.turret.TurretIOSim;
+import frc.robot.subsystems.shooter.turret.TurretIOSparkMax;
+import frc.robot.subsystems.vision.Vision;
 import frc.robot.util.AllianceFlipUtil;
 import frc.robot.util.FieldConstants;
 import java.util.List;
@@ -50,7 +59,7 @@ public class RobotContainer {
   private Guts leftGuts;
   private Guts rightGuts;
   private Intake intake;
-  // private Vision vision;
+  private Vision vision;
 
   public RobotContainer() {
     switch (Constants.kCurrentMode) {
@@ -63,21 +72,21 @@ public RobotContainer() {
                 new ModuleIOTalonFX(TunerConstants.BackLeft),
                 new ModuleIOTalonFX(TunerConstants.BackRight));
         // vision = new Vision(null, null);
-        // leftShooter =
-        //     new Shooter(
-        //         ShooterSide.LEFT,
-        //         new TurretIOSparkMax(DeviceIDs.kLeftTurretAzimuth),
-        //         new HoodIOSparkMax(DeviceIDs.kLeftTurretHood),
-        //         new FlywheelIOTalonFX(DeviceIDs.kLeftTurretFlywheel));
-        // rightShooter =
-        //     new Shooter(
-        //         ShooterSide.RIGHT,
-        //         new TurretIOSparkMax(DeviceIDs.kRightTurretAzimuth),
-        //         new HoodIOSparkMax(DeviceIDs.kRightTurretHood),
-        //         new FlywheelIOTalonFX(DeviceIDs.kRightTurretFlywheel));
-        // leftGuts = new Guts(GutSide.LEFT, new GutsIOSparkMax(DeviceIDs.kLeftGuts));
-        // rightGuts = new Guts(GutSide.RIGHT, new GutsIOSparkMax(DeviceIDs.kRightGuts));
-        // intake = new Intake(new IntakeIOHardware());
+        leftShooter =
+            new Shooter(
+                ShooterSide.LEFT,
+                new TurretIOSparkMax(ShooterSide.LEFT),
+                new HoodIOSparkMax(ShooterSide.LEFT),
+                new FlywheelIOTalonFX(ShooterSide.LEFT));
+        rightShooter =
+            new Shooter(
+                ShooterSide.RIGHT,
+                new TurretIOSparkMax(ShooterSide.RIGHT),
+                new HoodIOSparkMax(ShooterSide.RIGHT),
+                new FlywheelIOTalonFX(ShooterSide.RIGHT));
+        leftGuts = new Guts(GutSide.LEFT, new GutsIOSparkMax(DeviceIDs.kLeftGuts));
+        rightGuts = new Guts(GutSide.RIGHT, new GutsIOSparkMax(DeviceIDs.kRightGuts));
+        intake = new Intake(new IntakeIOHardware());
         break;
       case SIM:
         drive =
@@ -119,17 +128,8 @@ public RobotContainer() {
         // vision = new Vision(null, new CameraIO[] {});
         break;
     }
+    Leds.getInstance().solid(LedSection.ALL, Color.kCyan);
 
-    // if (Constants.kCurrentMode == Constants.Mode.REAL) {
-    // try {
-    // Constants.kRobotConfig = RobotConfig.fromGUISettings();
-    // } catch (Exception e) {
-    // // Handle exception as needed
-    // e.printStackTrace();
-    // }
-    // }
-
-    // configurePathPlanner();
     configureBindings();
   }
 
diff --git a/src/main/java/frc/robot/control/DriverControls.java b/src/main/java/frc/robot/control/DriverControls.java
index e08b2b9..246646d 100644
--- a/src/main/java/frc/robot/control/DriverControls.java
+++ b/src/main/java/frc/robot/control/DriverControls.java
@@ -1,8 +1,8 @@
 package frc.robot.control;
 
-import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.StartEndCommand;
 import frc.robot.commands.DriveCommands;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.guts.Guts;
@@ -82,18 +82,34 @@ public void configure() {
    */
   private void configureOneDriver() {
 
-    // driver
-    // .rightBumper()
-    // .and(this::isOneDriver)
-    // .onTrue(Shooter.shootBothAtHub(leftShooter, rightShooter));
-
-    // driver.leftBumper().and(this::isOneDriver).onTrue(intake.deploy().withTimeout(0.5));
-
-    driver.leftBumper().and(this::isOneDriver).onTrue(intake.retract().withTimeout(0.5));
+    driver
+        .rightBumper()
+        .and(this::isOneDriver)
+        .whileTrue(
+            new StartEndCommand(
+                    () -> {
+                      leftShooter.setFlywheelOpenLoop(0.75);
+                      rightShooter.setFlywheelOpenLoop(-0.75);
+                    },
+                    () -> {
+                      leftShooter.setFlywheelOpenLoop(0);
+                      rightShooter.setFlywheelOpenLoop(0);
+                    },
+                    leftShooter,
+                    rightShooter)
+                .alongWith(leftGuts.runGutForward(), rightGuts.runGutForward()));
+
+    driver.leftBumper().and(this::isOneDriver).whileTrue(intake.deploy());
+    driver.rightTrigger().and(this::isOneDriver).whileTrue(intake.retract());
+
+    // driver.leftBumper().and(this::isOneDriver).onTrue(intake.retract().withTimeout(0.5));
 
     driver.leftTrigger().and(this::isOneDriver).whileTrue(intake.intake());
 
-    driver.leftBumper().and(driver.rightBumper()).and(driver.yTriangle())
+    driver
+        .leftBumper()
+        .and(driver.rightBumper())
+        .and(driver.yTriangle())
         .onTrue(Commands.runOnce(() -> this.setMode(DriverMode.TWO_DRIVERS)));
   }
 
@@ -110,8 +126,7 @@ private void configureOneDriver() {
    *
    * 
thumb button: Driver Handoff
    */
-  private void configureTwoDrivers() {
-  }
+  private void configureTwoDrivers() {}
 
   private boolean isOneDriver() {
     return mode == DriverMode.ONE_DRIVER;
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
index d532999..4e323c8 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -1,7 +1,6 @@
 package frc.robot.subsystems.drive;
 
 import static edu.wpi.first.units.Units.Amps;
-import static edu.wpi.first.units.Units.Degrees;
 import static edu.wpi.first.units.Units.Inches;
 import static edu.wpi.first.units.Units.KilogramSquareMeters;
 import static edu.wpi.first.units.Units.MetersPerSecond;
@@ -11,7 +10,6 @@
 import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
-import com.ctre.phoenix6.configs.MountPoseConfigs;
 import com.ctre.phoenix6.configs.Pigeon2Configuration;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
@@ -138,9 +136,9 @@ public class TunerConstants {
                     .withStatorCurrentLimitEnable(true));
     private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
     // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
-    private static final Pigeon2Configuration pigeonConfigs =
-        new Pigeon2Configuration()
-            .withMountPose(new MountPoseConfigs().withMountPoseYaw(Degrees.of(-180)));
+    private static final Pigeon2Configuration pigeonConfigs = null;
+    // new Pigeon2Configuration()
+    //     .withMountPose(new MountPoseConfigs().withMountPoseYaw(Degrees.of(-180)));
 
     // CAN bus that the devices are located on;
     // All swerve devices must share the same CAN bus
diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
index 7f582a1..49fb299 100644
--- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
+++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
@@ -35,7 +35,7 @@ public GyroIOPigeon2() {
       pigeon.getConfigurator().apply(new Pigeon2Configuration());
     }
 
-    pigeon.getConfigurator().setYaw(0.0);
+    pigeon.getConfigurator().setYaw(180.0);
     yaw.setUpdateFrequency(DriveConstants.kOdometryFrequency);
     yawVelocity.setUpdateFrequency(50.0);
     pigeon.optimizeBusUtilization();
diff --git a/src/main/java/frc/robot/subsystems/guts/Guts.java b/src/main/java/frc/robot/subsystems/guts/Guts.java
index 9672738..2d84cfe 100644
--- a/src/main/java/frc/robot/subsystems/guts/Guts.java
+++ b/src/main/java/frc/robot/subsystems/guts/Guts.java
@@ -26,8 +26,7 @@ public class Guts extends SubsystemBase {
   public Guts(GutSide side, GutsIO io) {
     this.io = io;
     this.side = side;
-    speed =
-        (side == GutSide.LEFT) ? (GutsConstants.kGutMotorSpeed) : -(GutsConstants.kGutMotorSpeed);
+    speed = GutsConstants.kGutMotorSpeed;
   }
 
   /** Runs the gut motor forward at 0.5 speed, then stops it when finished. */
diff --git a/src/main/java/frc/robot/subsystems/guts/GutsConstants.java b/src/main/java/frc/robot/subsystems/guts/GutsConstants.java
index cfdbdcd..8fd42bc 100644
--- a/src/main/java/frc/robot/subsystems/guts/GutsConstants.java
+++ b/src/main/java/frc/robot/subsystems/guts/GutsConstants.java
@@ -2,7 +2,7 @@
 
 public final class GutsConstants {
 
-  public static final double kGutMotorSpeed = 0.5;
+  public static final double kGutMotorSpeed = 0.75;
   // Change Gear Ratio later
   public static final double kGutMotorGearRatio = 1.0;
 }
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
index 5196340..51fea0a 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -2,7 +2,7 @@
 
 public final class IntakeConstants {
   public static final double kPivotMotorSpeed = 0.5;
-  public static final double kRollerMotorSpeed = 0.5;
+  public static final double kRollerMotorSpeed = -0.5;
 
   // Change Gear Ratios later
   public static final double kPivotMotorGearRatio = 1.0;
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
index f8f2b86..abe8cbb 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
@@ -24,7 +24,7 @@ public static class IntakeIOInputs {
 
     public double pivotAppliedVolts = 0.0;
     public double wheelAppliedVolts = 0.0;
-    
+
     public double pivotCurrentDrawAmps = 0.0;
     public double wheelCurrentDrawAmps = 0.0;
   }
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java
index bcdb012..d0da33b 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java
@@ -20,15 +20,16 @@ public class IntakeIOHardware implements IntakeIO {
 
   public IntakeIOHardware() {
     pivotConfig = new SparkMaxConfig();
-    driveMotor.getConfigurator().apply(wheelMotorConfig);
+    // wheelMotorConfig = new TalonFXConfiguration();
+    // driveMotor.getConfigurator().apply(wheelMotorConfig);
     pivotMotor.configure(
         pivotConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
   }
 
   @Override
   public void setPivotPosition(double positionRad) {
-      // TODO Auto-generated method stub
-      IntakeIO.super.setPivotPosition(positionRad);
+    // TODO Auto-generated method stub
+    IntakeIO.super.setPivotPosition(positionRad);
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/leds/LedConstants.java b/src/main/java/frc/robot/subsystems/leds/LedConstants.java
new file mode 100644
index 0000000..2cce135
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/leds/LedConstants.java
@@ -0,0 +1,25 @@
+package frc.robot.subsystems.leds;
+
+public final class LedConstants {
+  public static final int kPort = 0;
+
+  public static final int kFullLength = 7;
+  public static final int kLeftTurretBottomLength = 7;
+  public static final int kRightTurretBottomLength = 0;
+  public static final int kLeftTurretTopLength = 0;
+  public static final int kRightTurretTopLength = 0;
+
+  public static final double kStartupBreathDuration = 1.0;
+  public static final double kStrobeSlowDuration = 0.2;
+  public static final double kBreatheFastDuration = 0.5;
+  public static final double kBreatheSlowDuration = 1.0;
+  public static final double kRainbowCycleLength = 25.0;
+  public static final double kRainbowDuration = 0.25;
+  public static final double kRainbowStrobeDuration = 0.2;
+  public static final double kWaveExponent = 0.4;
+  public static final double kWaveFastCycleLength = 25.0;
+  public static final double kWaveFastDuration = 0.25;
+  public static final double kWaveDisabledCycleLength = 15.0;
+  public static final double kWaveDisabledDuration = 2.0;
+  public static final double kStrobeDuration = 0.1;
+}
diff --git a/src/main/java/frc/robot/subsystems/leds/Leds.java b/src/main/java/frc/robot/subsystems/leds/Leds.java
new file mode 100644
index 0000000..31a5089
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/leds/Leds.java
@@ -0,0 +1,137 @@
+package frc.robot.subsystems.leds;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import java.util.List;
+
+public class Leds extends SubsystemBase {
+
+  private static Leds instance;
+
+  public static Leds getInstance() {
+    if (instance == null) instance = new Leds();
+    return instance;
+  }
+
+  private final AddressableLED leds = new AddressableLED(LedConstants.kPort);
+  private final AddressableLEDBuffer buffer = new AddressableLEDBuffer(LedConstants.kFullLength);
+
+  public record Section(int start, int end) {}
+
+  public enum LedSection {
+    ALL(new Section(0, LedConstants.kFullLength)),
+    ALL_LEFT(
+        new Section(
+            0, LedConstants.kLeftTurretBottomLength + LedConstants.kLeftTurretTopLength - 1)),
+    ALL_RIGHT(
+        new Section(
+            LedConstants.kLeftTurretBottomLength + LedConstants.kLeftTurretTopLength,
+            LedConstants.kFullLength)),
+    BOTTOM_LEFT_TURRET(new Section(0, LedConstants.kLeftTurretBottomLength - 1)),
+    TOP_LEFT_TURRET(
+        new Section(
+            LedConstants.kLeftTurretBottomLength,
+            LedConstants.kLeftTurretBottomLength + LedConstants.kLeftTurretTopLength - 1)),
+    TOP_RIGHT_TURRET(
+        new Section(
+            LedConstants.kLeftTurretBottomLength + LedConstants.kLeftTurretTopLength,
+            LedConstants.kLeftTurretBottomLength
+                + LedConstants.kLeftTurretTopLength
+                + LedConstants.kRightTurretTopLength
+                - 1)),
+    BOTTOM_RIGHT_TURRET(
+        new Section(
+            LedConstants.kLeftTurretBottomLength
+                + LedConstants.kLeftTurretTopLength
+                + LedConstants.kRightTurretTopLength,
+            LedConstants.kFullLength));
+
+    private final Section section;
+
+    private LedSection(Section section) {
+      this.section = section;
+    }
+
+    public Section getSection() {
+      return section;
+    }
+  }
+
+  public Leds() {
+    leds.setLength(buffer.getLength());
+    leds.setData(buffer);
+    leds.start();
+  }
+
+  @Override
+  public void periodic() {
+    // Default pattern (change this however you want)
+    solid(LedSection.ALL, Color.kAquamarine);
+  }
+
+  public void solid(LedSection section, Color color) {
+    for (int i = section.section.start(); i < section.section.end(); i++) {
+      buffer.setLED(i, color);
+    }
+  }
+
+  public void strobe(LedSection section, Color c1, Color c2, double duration) {
+    boolean useFirst = ((Timer.getTimestamp() % duration) / duration) > 0.5;
+    solid(section, useFirst ? c1 : c2);
+  }
+
+  public void breath(LedSection section, Color c1, Color c2, double duration) {
+    double x = ((Timer.getTimestamp() % duration) / duration) * 2.0 * Math.PI;
+    double ratio = (Math.sin(x) + 1.0) / 2.0;
+
+    Color mixed =
+        new Color(
+            c1.red * (1 - ratio) + c2.red * ratio,
+            c1.green * (1 - ratio) + c2.green * ratio,
+            c1.blue * (1 - ratio) + c2.blue * ratio);
+
+    solid(section, mixed);
+  }
+
+  public void rainbow(LedSection section, double cycleLength, double duration) {
+    double baseHue = (1 - ((Timer.getTimestamp() / duration) % 1.0)) * 180.0;
+    double huePerLed = 180.0 / cycleLength;
+
+    for (int i = section.section.start(); i < section.section.end(); i++) {
+      int hue = (int) ((baseHue + huePerLed * (i - section.section.start())) % 180);
+      buffer.setHSV(i, hue, 255, 255);
+    }
+  }
+
+  public void wave(LedSection section, Color c1, Color c2, double cycleLength, double duration) {
+    double x = (1 - ((Timer.getTimestamp() % duration) / duration)) * 2.0 * Math.PI;
+    double xDiff = (2.0 * Math.PI) / cycleLength;
+
+    for (int i = section.section.start(); i < section.section.end(); i++) {
+      double ratio = (Math.pow(Math.sin(x), LedConstants.kWaveExponent) + 1.0) / 2.0;
+
+      Color mixed =
+          new Color(
+              c1.red * (1 - ratio) + c2.red * ratio,
+              c1.green * (1 - ratio) + c2.green * ratio,
+              c1.blue * (1 - ratio) + c2.blue * ratio);
+
+      buffer.setLED(i, mixed);
+      x += xDiff;
+    }
+  }
+
+  public void stripes(LedSection section, List colors, int stripeLength, double duration) {
+    int offset =
+        (int) ((Timer.getTimestamp() % duration) / duration * stripeLength * colors.size());
+
+    for (int i = section.section.start(); i < section.section.end(); i++) {
+      int index =
+          (int) (Math.floor((double) (i - offset) / stripeLength) + colors.size()) % colors.size();
+      buffer.setLED(i, colors.get(index));
+    }
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index 6fa24bd..cf4cea0 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -13,72 +13,77 @@
 import frc.robot.util.GeomUtil;
 
 public final class ShooterConstants {
-    public static final double kLatencySeconds = 0.05;
-
-    public static final class TurretConstants {
-        public static final double kGearRatio = 10 / 1; // Motor / Turret
-        public static final double kMinTurretAngleRad = Units.degreesToRadians(-90);
-        public static final double kMaxTurretAngleRad = Units.degreesToRadians(90);
-        public static final double kAngleTolerance = Units.degreesToRadians(2);
-
-        public static final double kLeftMotorId = 12;
-        public static final double kRightMotorId = 13;
-
-        // +X = Forward, +Y = Left
-        public static final Transform3d kRobotToLeftTurret = new Transform3d(Inches.of(3.749), Inches.of(8.186),
-                Inches.of(13.401), Rotation3d.kZero);
-
-        public static final Transform3d kRobotToRightTurret = new Transform3d(Inches.of(3.749), Inches.of(-8.314),
-                Inches.of(13.401), Rotation3d.kZero);
-    }
-
-    public static final class HoodConstants {
-        public static final double kTurretToHoodInches = 1.878;
-        public static final double kGearRatio = 100 / 1;
-
-        public static final double kLeftHoodID = -1;
-        public static final double kRightHoodID = -1;
-
-        public static final double kAngleTolerance = Units.degreesToRadians(5);
-
-        public static final Transform3d kRobotToLeftHood = new Transform3d(
-                Inches.of(7.268715), Meters.of(0.20792316), Inches.of(16.018516), Rotation3d.kZero);
-
-        public static final Transform3d kRobotToRightHood = new Transform3d(
-                Inches.of(-7.270121),
-                Inches.of(-(12.062888 - (7.5 / 2.0))),
-                Inches.of(16.018516),
-                Rotation3d.kZero);
-
-        public static final Transform3d kLeftTurretToLeftHood = GeomUtil.toPose3d(HoodConstants.kRobotToLeftHood)
-                .minus(
-                        GeomUtil.toPose3d(TurretConstants.kRobotToLeftTurret)
-                                .plus(
-                                        new Transform3d(
-                                                Inches.of(7.268715), Inches.of(0), Inches.of(0), new Rotation3d())));
-
-        public static final Transform3d kRightTurretToRightHood = GeomUtil.toPose3d(HoodConstants.kRobotToRightHood)
-                .minus(
-                        GeomUtil.toPose3d(TurretConstants.kRobotToRightTurret)
-                                .plus(
-                                        new Transform3d(
-                                                Inches.of(-7.270121), Inches.of(0), Inches.of(0), new Rotation3d())));
-
-        public static final double kMinAngleRad = Units.degreesToRadians(0);
-        public static final double kMaxAngleRad = Units.degreesToRadians(30);
-    }
-
-    public static final class FlywheelConstants {
-        public static final double kGearRatio = 300;
-        public static final double kSpeedTolerance = 25.0;
-
-        public static final int kLeftFlywheelID = -1;
-        public static final int kRightFlywheelID = -1;
-
-        public static final Slot0Configs kGains = new Slot0Configs().withKP(0).withKI(0).withKD(0).withKS(0).withKV(0)
-                .withKA(0);
-        public static final MotorOutputConfigs kOutputConfigs = new MotorOutputConfigs()
-                .withNeutralMode(NeutralModeValue.Coast)
-                .withInverted(InvertedValue.Clockwise_Positive);
-    }
+  public static final double kLatencySeconds = 0.05;
+
+  public static final class TurretConstants {
+    public static final double kGearRatio = 10 / 1; // Motor / Turret
+    public static final double kMinTurretAngleRad = Units.degreesToRadians(-90);
+    public static final double kMaxTurretAngleRad = Units.degreesToRadians(90);
+    public static final double kAngleTolerance = Units.degreesToRadians(2);
+
+    public static final double kLeftMotorId = 12;
+    public static final double kRightMotorId = 13;
+
+    // +X = Forward, +Y = Left
+    public static final Transform3d kRobotToLeftTurret =
+        new Transform3d(Inches.of(3.749), Inches.of(8.186), Inches.of(13.401), Rotation3d.kZero);
+
+    public static final Transform3d kRobotToRightTurret =
+        new Transform3d(Inches.of(3.749), Inches.of(-8.314), Inches.of(13.401), Rotation3d.kZero);
+  }
+
+  public static final class HoodConstants {
+    public static final double kTurretToHoodInches = 1.878;
+    public static final double kGearRatio = 100 / 1;
+
+    public static final double kLeftHoodID = -1;
+    public static final double kRightHoodID = -1;
+
+    public static final double kAngleTolerance = Units.degreesToRadians(5);
+
+    public static final Transform3d kRobotToLeftHood =
+        new Transform3d(
+            Inches.of(7.268715), Meters.of(0.20792316), Inches.of(16.018516), Rotation3d.kZero);
+
+    public static final Transform3d kRobotToRightHood =
+        new Transform3d(
+            Inches.of(-7.270121),
+            Inches.of(-(12.062888 - (7.5 / 2.0))),
+            Inches.of(16.018516),
+            Rotation3d.kZero);
+
+    public static final Transform3d kLeftTurretToLeftHood =
+        GeomUtil.toPose3d(HoodConstants.kRobotToLeftHood)
+            .minus(
+                GeomUtil.toPose3d(TurretConstants.kRobotToLeftTurret)
+                    .plus(
+                        new Transform3d(
+                            Inches.of(7.268715), Inches.of(0), Inches.of(0), new Rotation3d())));
+
+    public static final Transform3d kRightTurretToRightHood =
+        GeomUtil.toPose3d(HoodConstants.kRobotToRightHood)
+            .minus(
+                GeomUtil.toPose3d(TurretConstants.kRobotToRightTurret)
+                    .plus(
+                        new Transform3d(
+                            Inches.of(-7.270121), Inches.of(0), Inches.of(0), new Rotation3d())));
+
+    public static final double kMinAngleRad = Units.degreesToRadians(0);
+    public static final double kMaxAngleRad = Units.degreesToRadians(30);
+  }
+
+  public static final class FlywheelConstants {
+    public static final double kGearRatio = 300;
+    public static final double kSpeedTolerance = 25.0;
+
+    public static final int kLeftFlywheelID = -1;
+    public static final int kRightFlywheelID = -1;
+
+    public static final Slot0Configs kGains =
+        new Slot0Configs().withKP(0).withKI(0).withKD(0).withKS(0).withKV(0).withKA(0);
+    public static final MotorOutputConfigs kOutputConfigs =
+        new MotorOutputConfigs()
+            .withNeutralMode(NeutralModeValue.Coast)
+            .withInverted(InvertedValue.Clockwise_Positive);
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java
index ee689ce..2fd2e32 100644
--- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java
@@ -30,25 +30,23 @@ public class FlywheelIOTalonFX implements FlywheelIO {
   private final VelocityVoltage velocityRequest = new VelocityVoltage(0).withSlot(0);
 
   public FlywheelIOTalonFX(ShooterSide side) {
-    motor = new TalonFX(
-        side == ShooterSide.LEFT
-            ? DeviceIDs.kLeftTurretFlywheel
-            : DeviceIDs.kRightTurretFlywheel);
-    motorConfig = new TalonFXConfiguration()
-        .withMotorOutput(
-            new MotorOutputConfigs()
-                .withInverted(
-                    side == ShooterSide.LEFT
-                        ? InvertedValue.Clockwise_Positive
-                        : InvertedValue.CounterClockwise_Positive))
-        .withSlot0(FlywheelConstants.kGains)
-        /**
-         * TODO: Update gains
-         * Peiwei, Ben: see the FlywheelConstants.kGains above... thats where the values are
-         * You also might have to check if the inverted values are correct, positive
-         * should spin the right way for shooting (line above that has the withInverted() method)
-         */
-        .withMotorOutput(FlywheelConstants.kOutputConfigs);
+    motor =
+        new TalonFX(
+            side == ShooterSide.LEFT
+                ? DeviceIDs.kLeftTurretFlywheel
+                : DeviceIDs.kRightTurretFlywheel);
+    motorConfig =
+        new TalonFXConfiguration()
+            .withMotorOutput(
+                new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive))
+            .withSlot0(FlywheelConstants.kGains)
+            /**
+             * TODO: Update gains Peiwei, Ben: see the FlywheelConstants.kGains above... thats where
+             * the values are You also might have to check if the inverted values are correct,
+             * positive should spin the right way for shooting (line above that has the
+             * withInverted() method)
+             */
+            .withMotorOutput(FlywheelConstants.kOutputConfigs);
     tryUntilOk(5, () -> motor.getConfigurator().apply(motorConfig, 0.25));
 
     velocitySignal = motor.getVelocity();
@@ -63,9 +61,10 @@ public FlywheelIOTalonFX(ShooterSide side) {
 
   @Override
   public void updateInputs(FlywheelIOInputs inputs) {
-    inputs.connected = BaseStatusSignal.refreshAll(
-        velocitySignal, accelerationSignal, voltageSignal, currentSignal)
-        .isOK();
+    inputs.connected =
+        BaseStatusSignal.refreshAll(
+                velocitySignal, accelerationSignal, voltageSignal, currentSignal)
+            .isOK();
     inputs.velocityRadPerSec = velocitySignal.getValue().in(RadiansPerSecond);
     inputs.appliedVolts = voltageSignal.getValueAsDouble();
     inputs.currentDrawAmps = currentSignal.getValueAsDouble();
diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
index e018154..c40eb6b 100644
--- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
@@ -1,8 +1,8 @@
 package frc.robot.subsystems.shooter.hood;
 
-import static frc.robot.util.SparkUtil.tryUntilOk;
 import static frc.robot.util.SparkUtil.ifOk;
 import static frc.robot.util.SparkUtil.sparkStickyFault;
+import static frc.robot.util.SparkUtil.tryUntilOk;
 
 import com.revrobotics.PersistMode;
 import com.revrobotics.RelativeEncoder;

From fb678b5bb3f89f27a11b1b6f313e24dd7732e4be Mon Sep 17 00:00:00 2001
From: Maxwell Morgan 
Date: Thu, 5 Mar 2026 16:44:48 -0500
Subject: [PATCH 45/61] Update

---
 src/main/java/frc/robot/Constants.java        |   1 +
 src/main/java/frc/robot/RobotContainer.java   |  25 +++-
 .../frc/robot/control/DriverControls.java     |   5 +
 .../subsystems/intake/IntakeConstants.java    |   2 +-
 .../robot/subsystems/leds/LedConstants.java   |  10 +-
 .../java/frc/robot/subsystems/leds/Leds.java  |  28 ++--
 .../frc/robot/subsystems/shooter/Shooter.java |  17 +++
 .../subsystems/shooter/ShooterConstants.java  |   2 +-
 .../subsystems/shooter/flywheel/Flywheel.java |   6 +
 .../frc/robot/util/LoggedTunableNumber.java   | 122 ++++++++++++++++++
 10 files changed, 194 insertions(+), 24 deletions(-)
 create mode 100644 src/main/java/frc/robot/util/LoggedTunableNumber.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index ffee2c5..d9c483a 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -37,6 +37,7 @@ public static enum Mode {
   public static final int kOperatorControllerPort = 1;
 
   public static boolean kDisableHAL = false;
+  public static boolean kTuningMode = true;
 
   public static void disableHAL() {
     kDisableHAL = true;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 5112164..f6b296d 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -16,6 +16,7 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.Constants.DeviceIDs;
 import frc.robot.RobotState.OdometryObservation;
+import frc.robot.commands.DriveCommands;
 import frc.robot.control.Configurable;
 import frc.robot.control.DefaultControls;
 import frc.robot.control.DriverController;
@@ -138,8 +139,8 @@ private void configureBindings() {
             new DefaultControls(driver, operator, drive, leftShooter, rightShooter),
             new DriverControls(
                 driver, operator, drive, leftShooter, rightShooter, leftGuts, rightGuts, intake)
-            //         // TODO: Implement ZoneControls
-            //         // ,new ZoneControls()
+            // // TODO: Implement ZoneControls
+            // // ,new ZoneControls()
             )
         .forEach(Configurable::configure);
   }
@@ -177,10 +178,22 @@ public void configurePathPlanner() {
         new InstantCommand(() -> RobotState.getInstance().resetRotation(Rotation2d.kZero)));
 
     NamedCommands.registerCommand(
-        "scoreBothShooters",
-        Shooter.shootBothAtTarget(
-            leftShooter,
-            rightShooter,
+        "Shoot Both At Hub",
+        Shooter.shootBothAtTargetNoTurret(
+                leftShooter,
+                rightShooter,
+                () -> AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d()))
+            .alongWith(leftGuts.runGutForward(), rightGuts.runGutForward()));
+
+    NamedCommands.registerCommand("Intake/Index Fuel", intake.intake());
+    NamedCommands.registerCommand("Deploy Intake", intake.intake());
+    NamedCommands.registerCommand("Retract Intake", intake.intake());
+
+    NamedCommands.registerCommand(
+        "Turn Robot To Hub",
+        DriveCommands.turnToPoint(
+            drive,
+            () -> RobotState.getInstance().getEstimatedPose(),
             () -> AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d())));
   }
 }
diff --git a/src/main/java/frc/robot/control/DriverControls.java b/src/main/java/frc/robot/control/DriverControls.java
index 246646d..8c7ad4e 100644
--- a/src/main/java/frc/robot/control/DriverControls.java
+++ b/src/main/java/frc/robot/control/DriverControls.java
@@ -2,6 +2,7 @@
 
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.StartEndCommand;
 import frc.robot.commands.DriveCommands;
 import frc.robot.subsystems.drive.Drive;
@@ -99,6 +100,10 @@ private void configureOneDriver() {
                     rightShooter)
                 .alongWith(leftGuts.runGutForward(), rightGuts.runGutForward()));
 
+    driver
+        .aCross()
+        .onTrue(new InstantCommand(() -> rightShooter.setFlywheelVelocity(1000), rightShooter));
+
     driver.leftBumper().and(this::isOneDriver).whileTrue(intake.deploy());
     driver.rightTrigger().and(this::isOneDriver).whileTrue(intake.retract());
 
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
index 51fea0a..648db63 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -2,7 +2,7 @@
 
 public final class IntakeConstants {
   public static final double kPivotMotorSpeed = 0.5;
-  public static final double kRollerMotorSpeed = -0.5;
+  public static final double kRollerMotorSpeed = -0.4;
 
   // Change Gear Ratios later
   public static final double kPivotMotorGearRatio = 1.0;
diff --git a/src/main/java/frc/robot/subsystems/leds/LedConstants.java b/src/main/java/frc/robot/subsystems/leds/LedConstants.java
index 2cce135..15a2e11 100644
--- a/src/main/java/frc/robot/subsystems/leds/LedConstants.java
+++ b/src/main/java/frc/robot/subsystems/leds/LedConstants.java
@@ -1,13 +1,13 @@
 package frc.robot.subsystems.leds;
 
 public final class LedConstants {
-  public static final int kPort = 0;
+  public static final int kPort = 1;
 
-  public static final int kFullLength = 7;
+  public static final int kFullLength = 10;
   public static final int kLeftTurretBottomLength = 7;
-  public static final int kRightTurretBottomLength = 0;
-  public static final int kLeftTurretTopLength = 0;
-  public static final int kRightTurretTopLength = 0;
+  public static final int kRightTurretBottomLength = 7;
+  public static final int kLeftTurretTopLength = 17;
+  public static final int kRightTurretTopLength = 15;
 
   public static final double kStartupBreathDuration = 1.0;
   public static final double kStrobeSlowDuration = 0.2;
diff --git a/src/main/java/frc/robot/subsystems/leds/Leds.java b/src/main/java/frc/robot/subsystems/leds/Leds.java
index 31a5089..02c47aa 100644
--- a/src/main/java/frc/robot/subsystems/leds/Leds.java
+++ b/src/main/java/frc/robot/subsystems/leds/Leds.java
@@ -9,10 +9,9 @@
 
 public class Leds extends SubsystemBase {
 
-  private static Leds instance;
+  private static final Leds instance = new Leds();
 
   public static Leds getInstance() {
-    if (instance == null) instance = new Leds();
     return instance;
   }
 
@@ -22,7 +21,7 @@ public static Leds getInstance() {
   public record Section(int start, int end) {}
 
   public enum LedSection {
-    ALL(new Section(0, LedConstants.kFullLength)),
+    ALL(new Section(0, LedConstants.kFullLength - 1)),
     ALL_LEFT(
         new Section(
             0, LedConstants.kLeftTurretBottomLength + LedConstants.kLeftTurretTopLength - 1)),
@@ -60,7 +59,7 @@ public Section getSection() {
     }
   }
 
-  public Leds() {
+  private Leds() {
     leds.setLength(buffer.getLength());
     leds.setData(buffer);
     leds.start();
@@ -68,12 +67,16 @@ public Leds() {
 
   @Override
   public void periodic() {
-    // Default pattern (change this however you want)
-    solid(LedSection.ALL, Color.kAquamarine);
+    solid(LedSection.ALL, Color.kAqua);
+    // solid(LedSection.TOP_LEFT_TURRET, Color.kLimeGreen);
+    // solid(LedSection.BOTTOM_LEFT_TURRET, Color.kYellow);
+    // solid(LedSection.BOTTOM_RIGHT_TURRET, Color.kSkyBlue);
+    leds.setData(buffer);
   }
 
   public void solid(LedSection section, Color color) {
-    for (int i = section.section.start(); i < section.section.end(); i++) {
+    Section s = section.getSection();
+    for (int i = s.start(); i < s.end(); i++) {
       buffer.setLED(i, color);
     }
   }
@@ -97,20 +100,22 @@ public void breath(LedSection section, Color c1, Color c2, double duration) {
   }
 
   public void rainbow(LedSection section, double cycleLength, double duration) {
+    Section s = section.getSection();
     double baseHue = (1 - ((Timer.getTimestamp() / duration) % 1.0)) * 180.0;
     double huePerLed = 180.0 / cycleLength;
 
-    for (int i = section.section.start(); i < section.section.end(); i++) {
-      int hue = (int) ((baseHue + huePerLed * (i - section.section.start())) % 180);
+    for (int i = s.start(); i < s.end(); i++) {
+      int hue = (int) ((baseHue + huePerLed * (i - s.start())) % 180);
       buffer.setHSV(i, hue, 255, 255);
     }
   }
 
   public void wave(LedSection section, Color c1, Color c2, double cycleLength, double duration) {
+    Section s = section.getSection();
     double x = (1 - ((Timer.getTimestamp() % duration) / duration)) * 2.0 * Math.PI;
     double xDiff = (2.0 * Math.PI) / cycleLength;
 
-    for (int i = section.section.start(); i < section.section.end(); i++) {
+    for (int i = s.start(); i < s.end(); i++) {
       double ratio = (Math.pow(Math.sin(x), LedConstants.kWaveExponent) + 1.0) / 2.0;
 
       Color mixed =
@@ -125,10 +130,11 @@ public void wave(LedSection section, Color c1, Color c2, double cycleLength, dou
   }
 
   public void stripes(LedSection section, List colors, int stripeLength, double duration) {
+    Section s = section.getSection();
     int offset =
         (int) ((Timer.getTimestamp() % duration) / duration * stripeLength * colors.size());
 
-    for (int i = section.section.start(); i < section.section.end(); i++) {
+    for (int i = s.start(); i < s.end(); i++) {
       int index =
           (int) (Math.floor((double) (i - offset) / stripeLength) + colors.size()) % colors.size();
       buffer.setLED(i, colors.get(index));
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index db5386a..3ce7f6e 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -69,6 +69,18 @@ public static Command shootBothAtTarget(
         rightShooter);
   }
 
+  public static Command shootBothAtTargetNoTurret(
+      Shooter leftShooter, Shooter rightShooter, Supplier targetSupplier) {
+    return Commands.run(
+        () -> {
+          var cmds = TrajectoryCalculator.calculateBoth(targetSupplier.get());
+          leftShooter.applyCommandNoRotation(cmds.left());
+          rightShooter.applyCommandNoRotation(cmds.right());
+        },
+        leftShooter,
+        rightShooter);
+  }
+
   /**
    * Apply a pre-calculated shooter command to this shooter. This does not require the shooter
    * subsystem - use when combining with other shooters.
@@ -81,6 +93,11 @@ public void applyCommand(ShooterCommand cmd) {
     turret.setPosition(cmd.turretAngle());
   }
 
+  public void applyCommandNoRotation(ShooterCommand cmd) {
+    flywheel.setVelocity(cmd.wheelRPM());
+    hood.setAngle(cmd.hoodAngle());
+  }
+
   public Command shootAtTarget(Supplier targetSupplier) {
     return Commands.run(
         () -> {
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index cf4cea0..9335506 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -80,7 +80,7 @@ public static final class FlywheelConstants {
     public static final int kRightFlywheelID = -1;
 
     public static final Slot0Configs kGains =
-        new Slot0Configs().withKP(0).withKI(0).withKD(0).withKS(0).withKV(0).withKA(0);
+        new Slot0Configs().withKP(0).withKI(0).withKD(0).withKS(0.1).withKV(0).withKA(0);
     public static final MotorOutputConfigs kOutputConfigs =
         new MotorOutputConfigs()
             .withNeutralMode(NeutralModeValue.Coast)
diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java
index 1976ded..5f9e4ea 100644
--- a/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java
+++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java
@@ -7,6 +7,7 @@
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.subsystems.shooter.Shooter.ShooterSide;
 import frc.robot.subsystems.shooter.ShooterConstants.FlywheelConstants;
@@ -20,6 +21,7 @@ public class Flywheel extends SubsystemBase {
 
   private boolean atGoal = false;
   private Debouncer atGoalDebouncer = new Debouncer(0.2, DebounceType.kFalling);
+  private double goalRPM = 0.0;
 
   /** Creates a new Flywheel. */
   public Flywheel(ShooterSide side, FlywheelIO io) {
@@ -32,9 +34,13 @@ public void periodic() {
     io.updateInputs(inputs);
     Logger.processInputs("Shooter/" + side.getName() + "/Flywheel", inputs);
     Logger.recordOutput("Shooter/" + side.getName() + "/Flywheel/AtGoal", atGoal);
+
+    SmartDashboard.putNumber("Flywheel Velo", getVelocity());
+    SmartDashboard.putNumber("Flywheel Setpoint", goalRPM);
   }
 
   public void setVelocity(double velocityRPM) {
+    goalRPM = velocityRPM;
     atGoal =
         atGoalDebouncer.calculate(
             Math.abs(
diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java
new file mode 100644
index 0000000..75c835b
--- /dev/null
+++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java
@@ -0,0 +1,122 @@
+// Copyright (c) 2025-2026 Littleton Robotics
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+package frc.robot.util;
+
+import frc.robot.Constants;
+import java.util.Arrays;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.function.Consumer;
+import java.util.function.DoubleSupplier;
+import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
+
+/**
+ * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or
+ * value not in dashboard.
+ */
+public class LoggedTunableNumber implements DoubleSupplier {
+  private static final String tableKey = "/Tuning";
+
+  private final String key;
+  private boolean hasDefault = false;
+  private double defaultValue;
+  private LoggedNetworkNumber dashboardNumber;
+  private Map lastHasChangedValues = new HashMap<>();
+
+  /**
+   * Create a new LoggedTunableNumber
+   *
+   * @param dashboardKey Key on dashboard
+   */
+  public LoggedTunableNumber(String dashboardKey) {
+    this.key = tableKey + "/" + dashboardKey;
+  }
+
+  /**
+   * Create a new LoggedTunableNumber with the default value
+   *
+   * @param dashboardKey Key on dashboard
+   * @param defaultValue Default value
+   */
+  public LoggedTunableNumber(String dashboardKey, double defaultValue) {
+    this(dashboardKey);
+    initDefault(defaultValue);
+  }
+
+  /**
+   * Set the default value of the number. The default value can only be set once.
+   *
+   * @param defaultValue The default value
+   */
+  public void initDefault(double defaultValue) {
+    if (!hasDefault) {
+      hasDefault = true;
+      this.defaultValue = defaultValue;
+      if (Constants.kTuningMode && !Constants.kDisableHAL) {
+        dashboardNumber = new LoggedNetworkNumber(key, defaultValue);
+      }
+    }
+  }
+
+  /**
+   * Get the current value, from dashboard if available and in tuning mode.
+   *
+   * @return The current value
+   */
+  public double get() {
+    if (!hasDefault) {
+      return 0.0;
+    } else {
+      return Constants.kTuningMode && !Constants.kDisableHAL ? dashboardNumber.get() : defaultValue;
+    }
+  }
+
+  /**
+   * Checks whether the number has changed since our last check
+   *
+   * @param id Unique identifier for the caller to avoid conflicts when shared between multiple
+   *     objects. Recommended approach is to pass the result of "hashCode()"
+   * @return True if the number has changed since the last time this method was called, false
+   *     otherwise.
+   */
+  public boolean hasChanged(int id) {
+    double currentValue = get();
+    Double lastValue = lastHasChangedValues.get(id);
+    if (lastValue == null || currentValue != lastValue) {
+      lastHasChangedValues.put(id, currentValue);
+      return true;
+    }
+
+    return false;
+  }
+
+  /**
+   * Runs action if any of the tunableNumbers have changed
+   *
+   * @param id Unique identifier for the caller to avoid conflicts when shared between multiple *
+   *     objects. Recommended approach is to pass the result of "hashCode()"
+   * @param action Callback to run when any of the tunable numbers have changed. Access tunable
+   *     numbers in order inputted in method
+   * @param tunableNumbers All tunable numbers to check
+   */
+  public static void ifChanged(
+      int id, Consumer action, LoggedTunableNumber... tunableNumbers) {
+    if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) {
+      action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray());
+    }
+  }
+
+  /** Runs action if any of the tunableNumbers have changed */
+  public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) {
+    ifChanged(id, values -> action.run(), tunableNumbers);
+  }
+
+  @Override
+  public double getAsDouble() {
+    return get();
+  }
+}

From af4c7aab4259dc551075aba5b9e0679e911f5c74 Mon Sep 17 00:00:00 2001
From: Maxwell Morgan 
Date: Fri, 6 Mar 2026 18:02:52 -0500
Subject: [PATCH 46/61] In theory it should all work

---
 src/main/java/frc/robot/RobotContainer.java   |  28 ++++-
 src/main/java/frc/robot/RobotState.java       |   5 +-
 .../frc/robot/control/DefaultControls.java    |   5 +-
 .../frc/robot/control/DriverControls.java     | 114 +++++++++++-------
 .../robot/subsystems/drive/GyroIOPigeon2.java |   2 +-
 .../frc/robot/subsystems/intake/Intake.java   |   7 ++
 .../subsystems/intake/IntakeConstants.java    |   3 +-
 .../frc/robot/subsystems/shooter/Shooter.java |   4 +-
 .../subsystems/shooter/ShooterConstants.java  |   8 +-
 .../subsystems/shooter/flywheel/Flywheel.java |  13 ++
 .../shooter/flywheel/FlywheelIO.java          |   2 +-
 .../shooter/flywheel/FlywheelIOTalonFX.java   |  22 ++--
 12 files changed, 147 insertions(+), 66 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index f6b296d..f73eb92 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -8,14 +8,18 @@
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.config.PIDConstants;
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.Constants.DeviceIDs;
 import frc.robot.RobotState.OdometryObservation;
+import frc.robot.RobotState.VisionMeasurement;
 import frc.robot.commands.DriveCommands;
 import frc.robot.control.Configurable;
 import frc.robot.control.DefaultControls;
@@ -45,14 +49,16 @@
 import frc.robot.subsystems.shooter.hood.HoodIOSparkMax;
 import frc.robot.subsystems.shooter.turret.TurretIOSim;
 import frc.robot.subsystems.shooter.turret.TurretIOSparkMax;
+import frc.robot.subsystems.vision.CameraIOLimelight;
 import frc.robot.subsystems.vision.Vision;
+import frc.robot.subsystems.vision.Vision.VisionConsumer;
 import frc.robot.util.AllianceFlipUtil;
 import frc.robot.util.FieldConstants;
 import java.util.List;
 
 public class RobotContainer {
   private final DriverController driver = new DriverController.XboxDriverController(0);
-  private final Joystick operator = new Joystick(Constants.kOperatorControllerPort);
+  private final DriverController operator = new DriverController.XboxDriverController(1);
 
   private Drive drive;
   private Shooter leftShooter;
@@ -88,6 +94,24 @@ public RobotContainer() {
         leftGuts = new Guts(GutSide.LEFT, new GutsIOSparkMax(DeviceIDs.kLeftGuts));
         rightGuts = new Guts(GutSide.RIGHT, new GutsIOSparkMax(DeviceIDs.kRightGuts));
         intake = new Intake(new IntakeIOHardware());
+        vision =
+            new Vision(
+                new VisionConsumer() {
+                  @Override
+                  public void accept(
+                      Pose2d visionRobotPoseMeters,
+                      double timestampSeconds,
+                      Matrix visionMeasurementStdDevs) {
+                    RobotState.getInstance()
+                        .addVisionMeasurement(
+                            new VisionMeasurement(
+                                timestampSeconds, visionRobotPoseMeters, visionMeasurementStdDevs));
+                  }
+                },
+                new CameraIOLimelight(
+                    "limelight-front", () -> RobotState.getInstance().getRotation()),
+                new CameraIOLimelight(
+                    "limelight-left", () -> RobotState.getInstance().getRotation()));
         break;
       case SIM:
         drive =
diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java
index 44b28c6..4420459 100644
--- a/src/main/java/frc/robot/RobotState.java
+++ b/src/main/java/frc/robot/RobotState.java
@@ -3,7 +3,6 @@
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 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.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -67,7 +66,7 @@ public void addOdometryObservation(OdometryObservation observation) {
    */
   public void addVisionMeasurement(VisionMeasurement measurement) {
     poseEstimator.addVisionMeasurement(
-        measurement.visionPose().toPose2d(), measurement.timestamp(), measurement.stdDevs());
+        measurement.visionPose(), measurement.timestamp(), measurement.stdDevs());
 
     Logger.recordOutput("RobotState/EstimatedPose", poseEstimator.getEstimatedPosition());
   }
@@ -136,5 +135,5 @@ public ChassisSpeeds getFieldVelocity() {
   public record OdometryObservation(
       double timestamp, SwerveModulePosition[] modulePositions, Rotation2d gyroAngle) {}
 
-  public record VisionMeasurement(double timestamp, Pose3d visionPose, Matrix stdDevs) {}
+  public record VisionMeasurement(double timestamp, Pose2d visionPose, Matrix stdDevs) {}
 }
diff --git a/src/main/java/frc/robot/control/DefaultControls.java b/src/main/java/frc/robot/control/DefaultControls.java
index 66ff50d..b42b0ae 100644
--- a/src/main/java/frc/robot/control/DefaultControls.java
+++ b/src/main/java/frc/robot/control/DefaultControls.java
@@ -1,6 +1,5 @@
 package frc.robot.control;
 
-import edu.wpi.first.wpilibj.Joystick;
 import frc.robot.commands.DriveCommands;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.shooter.Shooter;
@@ -8,7 +7,7 @@
 public class DefaultControls implements Configurable {
 
   private final DriverController driver;
-  private final Joystick operator;
+  private final DriverController operator;
   private final Drive drive;
   private final Shooter leftShooter;
   private final Shooter rightShooter;
@@ -16,7 +15,7 @@ public class DefaultControls implements Configurable {
   /** Creates a new DefaultControls. */
   public DefaultControls(
       DriverController driver,
-      Joystick operator,
+      DriverController operator,
       Drive drive,
       Shooter leftShooter,
       Shooter rightShooter) {
diff --git a/src/main/java/frc/robot/control/DriverControls.java b/src/main/java/frc/robot/control/DriverControls.java
index 8c7ad4e..38073cf 100644
--- a/src/main/java/frc/robot/control/DriverControls.java
+++ b/src/main/java/frc/robot/control/DriverControls.java
@@ -1,15 +1,20 @@
 package frc.robot.control;
 
-import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.StartEndCommand;
+import frc.robot.RobotState;
 import frc.robot.commands.DriveCommands;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.guts.Guts;
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.util.AllianceFlipUtil;
 import frc.robot.util.Direction;
+import frc.robot.util.FieldConstants;
+import frc.robot.util.FieldConstants.Hub;
 import org.littletonrobotics.junction.AutoLogOutput;
 
 public class DriverControls implements Configurable {
@@ -22,7 +27,7 @@ public enum DriverMode {
   }
 
   private final DriverController driver;
-  private final Joystick operator;
+  private final DriverController operator;
   private final Drive drive;
   private final Shooter leftShooter;
   private final Shooter rightShooter;
@@ -32,7 +37,7 @@ public enum DriverMode {
 
   public DriverControls(
       DriverController driver,
-      Joystick operator,
+      DriverController operator,
       Drive drive,
       Shooter leftShooter,
       Shooter rightShooter,
@@ -53,7 +58,8 @@ public DriverControls(
   public void configure() {
 
     // Neutral controls (regardless of whether we are in one or two driver mode)
-    driver.xSquare().onTrue(Commands.runOnce(drive::zeroYaw));
+    driver.xSquare().onTrue(Commands.runOnce(drive::zeroYaw, drive));
+    driver.bCircle().onTrue(Commands.runOnce(drive::stopWithX, drive));
 
     driver.dPadUp().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTH));
     driver.dPadUpLeft().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTHWEST));
@@ -64,8 +70,65 @@ public void configure() {
     driver.dPadDownRight().whileTrue(DriveCommands.crabWalk(drive, Direction.SOUTHEAST));
     driver.dPadDown().whileTrue(DriveCommands.crabWalk(drive, Direction.SOUTH));
 
-    configureOneDriver();
-    configureTwoDrivers();
+    driver
+        .leftBumper()
+        .whileTrue(
+            DriveCommands.joystickDriveAtAngle(
+                drive,
+                () -> -driver.getLeftY(), // xSupplier
+                () -> -driver.getLeftX(), // ySupplier
+                () -> {
+                  Pose2d robotPose = RobotState.getInstance().getEstimatedPose();
+                  Translation2d target =
+                      AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d());
+
+                  Translation2d delta = target.minus(robotPose.getTranslation());
+
+                  return new Rotation2d(Math.atan2(delta.getY(), delta.getX()));
+                }));
+
+    operator.leftBumper().and(operator.leftTrigger().negate()).whileTrue(intake.intake());
+
+    operator
+        .rightBumper()
+        .whileTrue(
+            rightShooter
+                .setFlywheelVelocity(7000)
+                .alongWith(leftShooter.setFlywheelVelocity(7000)));
+
+    operator.rightBumper().whileTrue(leftGuts.runGutForward());
+    operator.rightBumper().whileTrue(rightGuts.runGutForward());
+
+    operator
+        .dPadUp()
+        .whileTrue(
+            new StartEndCommand(
+                () -> {
+                  leftShooter.setHoodOpenLoop(0.01);
+                  rightShooter.setHoodOpenLoop(0.01);
+                },
+                () -> {
+                  leftShooter.setHoodOpenLoop(0);
+                  rightShooter.setHoodOpenLoop(0);
+                }));
+
+    operator
+        .dPadDown()
+        .whileTrue(
+            new StartEndCommand(
+                () -> {
+                  leftShooter.setHoodOpenLoop(-0.01);
+                  rightShooter.setHoodOpenLoop(-0.01);
+                },
+                () -> {
+                  leftShooter.setHoodOpenLoop(0);
+                  rightShooter.setHoodOpenLoop(0);
+                }));
+
+    operator.leftTrigger().whileTrue(intake.intakeSignificantlyFaster());
+    operator.aCross().whileTrue(intake.outtake());
+    operator.xSquare().whileTrue(intake.deploy());
+    operator.yTriangle().whileTrue(intake.retract());
   }
 
   /*
@@ -81,42 +144,7 @@ public void configure() {
    * LB + RB + Y: Aux Handoff
    *
    */
-  private void configureOneDriver() {
-
-    driver
-        .rightBumper()
-        .and(this::isOneDriver)
-        .whileTrue(
-            new StartEndCommand(
-                    () -> {
-                      leftShooter.setFlywheelOpenLoop(0.75);
-                      rightShooter.setFlywheelOpenLoop(-0.75);
-                    },
-                    () -> {
-                      leftShooter.setFlywheelOpenLoop(0);
-                      rightShooter.setFlywheelOpenLoop(0);
-                    },
-                    leftShooter,
-                    rightShooter)
-                .alongWith(leftGuts.runGutForward(), rightGuts.runGutForward()));
-
-    driver
-        .aCross()
-        .onTrue(new InstantCommand(() -> rightShooter.setFlywheelVelocity(1000), rightShooter));
-
-    driver.leftBumper().and(this::isOneDriver).whileTrue(intake.deploy());
-    driver.rightTrigger().and(this::isOneDriver).whileTrue(intake.retract());
-
-    // driver.leftBumper().and(this::isOneDriver).onTrue(intake.retract().withTimeout(0.5));
-
-    driver.leftTrigger().and(this::isOneDriver).whileTrue(intake.intake());
-
-    driver
-        .leftBumper()
-        .and(driver.rightBumper())
-        .and(driver.yTriangle())
-        .onTrue(Commands.runOnce(() -> this.setMode(DriverMode.TWO_DRIVERS)));
-  }
+  private void configureOneDriver() {}
 
   /*
    * 
Back up Operator Controls:
diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
index 49fb299..d59493c 100644
--- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
+++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
@@ -35,7 +35,7 @@ public GyroIOPigeon2() {
       pigeon.getConfigurator().apply(new Pigeon2Configuration());
     }
 
-    pigeon.getConfigurator().setYaw(180.0);
+    pigeon.setYaw(180.0);
     yaw.setUpdateFrequency(DriveConstants.kOdometryFrequency);
     yawVelocity.setUpdateFrequency(50.0);
     pigeon.optimizeBusUtilization();
diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
index d227d65..22413d0 100644
--- a/src/main/java/frc/robot/subsystems/intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -55,6 +55,13 @@ public Command intake() {
         this);
   }
 
+  public Command intakeSignificantlyFaster() {
+    return Commands.runEnd(
+        () -> io.setWheelSpeed(IntakeConstants.kRollerMotorSpeed),
+        () -> io.setWheelSpeed(0.0),
+        this);
+  }
+
   /**
    * Command to run the feeder backward
    *
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
index 648db63..ca2491c 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -2,7 +2,8 @@
 
 public final class IntakeConstants {
   public static final double kPivotMotorSpeed = 0.5;
-  public static final double kRollerMotorSpeed = -0.4;
+  public static final double kRollerMotorSpeed = -0.5;
+  public static final double kSignificantlyFasterRollerMotorSpeed = -0.75;
 
   // Change Gear Ratios later
   public static final double kPivotMotorGearRatio = 1.0;
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 3ce7f6e..c396220 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -120,8 +120,8 @@ public Command zeroTurret() {
     return turret.zero();
   }
 
-  public void setFlywheelVelocity(double velocityRPM) {
-    flywheel.setVelocity(velocityRPM);
+  public Command setFlywheelVelocity(double velocityRPM) {
+    return flywheel.runVelocity(velocityRPM);
   }
 
   public void setHoodAngle(double angle) {
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index 9335506..a2d8b07 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -80,7 +80,13 @@ public static final class FlywheelConstants {
     public static final int kRightFlywheelID = -1;
 
     public static final Slot0Configs kGains =
-        new Slot0Configs().withKP(0).withKI(0).withKD(0).withKS(0.1).withKV(0).withKA(0);
+        new Slot0Configs()
+            .withKP(0.75)
+            .withKI(0)
+            .withKD(0.0)
+            .withKS(0.0225 * 12)
+            .withKV(0.0945)
+            .withKA(0);
     public static final MotorOutputConfigs kOutputConfigs =
         new MotorOutputConfigs()
             .withNeutralMode(NeutralModeValue.Coast)
diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java
index 5f9e4ea..6cdd1c4 100644
--- a/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java
+++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java
@@ -8,6 +8,8 @@
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.math.util.Units;
 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.SubsystemBase;
 import frc.robot.subsystems.shooter.Shooter.ShooterSide;
 import frc.robot.subsystems.shooter.ShooterConstants.FlywheelConstants;
@@ -39,6 +41,17 @@ public void periodic() {
     SmartDashboard.putNumber("Flywheel Setpoint", goalRPM);
   }
 
+  public Command runVelocity(double velocityRPM) {
+    return Commands.startEnd(
+        () -> {
+          setVelocity(velocityRPM);
+        },
+        () -> {
+          stop();
+        },
+        this);
+  }
+
   public void setVelocity(double velocityRPM) {
     goalRPM = velocityRPM;
     atGoal =
diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java
index 79816e7..974a9e3 100644
--- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java
+++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java
@@ -16,7 +16,7 @@ public static class FlywheelIOInputs {
   /**
    * Set the shooter motor to a specified velocity.
    *
-   * @param velocity The velocity to set the motor to (in RPM).
+   * @param velocity The velocity to set the motor to (in RPS).
    */
   default void setVelocity(double velocity) {}
 
diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java
index 2fd2e32..6ace554 100644
--- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java
@@ -21,6 +21,7 @@
 public class FlywheelIOTalonFX implements FlywheelIO {
   private final TalonFX motor;
   private final TalonFXConfiguration motorConfig;
+  private final ShooterSide side;
 
   private final StatusSignal velocitySignal;
   private final StatusSignal accelerationSignal;
@@ -30,6 +31,7 @@ public class FlywheelIOTalonFX implements FlywheelIO {
   private final VelocityVoltage velocityRequest = new VelocityVoltage(0).withSlot(0);
 
   public FlywheelIOTalonFX(ShooterSide side) {
+    this.side = side;
     motor =
         new TalonFX(
             side == ShooterSide.LEFT
@@ -38,15 +40,17 @@ public FlywheelIOTalonFX(ShooterSide side) {
     motorConfig =
         new TalonFXConfiguration()
             .withMotorOutput(
-                new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive))
-            .withSlot0(FlywheelConstants.kGains)
-            /**
-             * TODO: Update gains Peiwei, Ben: see the FlywheelConstants.kGains above... thats where
-             * the values are You also might have to check if the inverted values are correct,
-             * positive should spin the right way for shooting (line above that has the
-             * withInverted() method)
-             */
-            .withMotorOutput(FlywheelConstants.kOutputConfigs);
+                new MotorOutputConfigs()
+                    .withInverted(
+                        side == ShooterSide.LEFT
+                            ? InvertedValue.Clockwise_Positive
+                            : InvertedValue.CounterClockwise_Positive))
+            .withSlot0(FlywheelConstants.kGains);
+    /**
+     * TODO: Update gains Peiwei, Ben: see the FlywheelConstants.kGains above... thats where the
+     * values are You also might have to check if the inverted values are correct, positive should
+     * spin the right way for shooting (line above that has the withInverted() method)
+     */
     tryUntilOk(5, () -> motor.getConfigurator().apply(motorConfig, 0.25));
 
     velocitySignal = motor.getVelocity();

From e673d6ddf9b94b0378eb301d5c237fbea3077bc3 Mon Sep 17 00:00:00 2001
From: Sim-City 
Date: Sat, 7 Mar 2026 09:02:43 -0500
Subject: [PATCH 47/61] Update for comp

---
 .../apriltags/2026-rebuilt-andymark.json      | 584 ++++++++++++++++++
 src/main/java/frc/robot/Constants.java        |  12 +-
 src/main/java/frc/robot/RobotContainer.java   |   5 +-
 .../frc/robot/control/DriverControls.java     |  19 +-
 .../robot/subsystems/drive/GyroIOPigeon2.java |   5 +-
 .../subsystems/intake/IntakeConstants.java    |   2 +-
 .../robot/subsystems/leds/LedConstants.java   |   2 +-
 .../frc/robot/subsystems/shooter/Shooter.java |  35 +-
 .../shooter/flywheel/FlywheelIOTalonFX.java   |  18 +-
 .../shooter/hood/HoodIOSparkMax.java          |   2 +-
 .../shooter/turret/TurretIOSparkMax.java      |   2 +-
 11 files changed, 643 insertions(+), 43 deletions(-)
 create mode 100644 src/main/deploy/apriltags/2026-rebuilt-andymark.json

diff --git a/src/main/deploy/apriltags/2026-rebuilt-andymark.json b/src/main/deploy/apriltags/2026-rebuilt-andymark.json
new file mode 100644
index 0000000..ecc0390
--- /dev/null
+++ b/src/main/deploy/apriltags/2026-rebuilt-andymark.json
@@ -0,0 +1,584 @@
+{
+  "tags": [
+    {
+      "ID": 1,
+      "pose": {
+        "translation": {
+          "x": 11.863959,
+          "y": 7.411491399999999,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 2,
+      "pose": {
+        "translation": {
+          "x": 11.9013986,
+          "y": 4.6247558,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 3,
+      "pose": {
+        "translation": {
+          "x": 11.2978438,
+          "y": 4.3769534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 4,
+      "pose": {
+        "translation": {
+          "x": 11.2978438,
+          "y": 4.0213534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 5,
+      "pose": {
+        "translation": {
+          "x": 11.9013986,
+          "y": 3.417951,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 6,
+      "pose": {
+        "translation": {
+          "x": 11.863959,
+          "y": 0.6312154,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 7,
+      "pose": {
+        "translation": {
+          "x": 11.9388636,
+          "y": 0.6312154,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 8,
+      "pose": {
+        "translation": {
+          "x": 12.2569986,
+          "y": 3.417951,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 9,
+      "pose": {
+        "translation": {
+          "x": 12.5051566,
+          "y": 3.6657534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 10,
+      "pose": {
+        "translation": {
+          "x": 12.5051566,
+          "y": 4.0213534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 11,
+      "pose": {
+        "translation": {
+          "x": 12.2569986,
+          "y": 4.6247558,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 12,
+      "pose": {
+        "translation": {
+          "x": 11.9388636,
+          "y": 7.411491399999999,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 13,
+      "pose": {
+        "translation": {
+          "x": 16.499332,
+          "y": 7.391907999999999,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 14,
+      "pose": {
+        "translation": {
+          "x": 16.499332,
+          "y": 6.960107999999999,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 15,
+      "pose": {
+        "translation": {
+          "x": 16.4989764,
+          "y": 4.3124882,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 16,
+      "pose": {
+        "translation": {
+          "x": 16.4989764,
+          "y": 3.8806881999999994,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 17,
+      "pose": {
+        "translation": {
+          "x": 4.6490636,
+          "y": 0.6312154,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 18,
+      "pose": {
+        "translation": {
+          "x": 4.6115986,
+          "y": 3.417951,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 19,
+      "pose": {
+        "translation": {
+          "x": 5.2151534,
+          "y": 3.6657534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 20,
+      "pose": {
+        "translation": {
+          "x": 5.2151534,
+          "y": 4.0213534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 21,
+      "pose": {
+        "translation": {
+          "x": 4.6115986,
+          "y": 4.6247558,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 22,
+      "pose": {
+        "translation": {
+          "x": 4.6490636,
+          "y": 7.411491399999999,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 23,
+      "pose": {
+        "translation": {
+          "x": 4.574159,
+          "y": 7.411491399999999,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 24,
+      "pose": {
+        "translation": {
+          "x": 4.2559986,
+          "y": 4.6247558,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 25,
+      "pose": {
+        "translation": {
+          "x": 4.007866,
+          "y": 4.3769534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 26,
+      "pose": {
+        "translation": {
+          "x": 4.007866,
+          "y": 4.0213534,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 27,
+      "pose": {
+        "translation": {
+          "x": 4.2559986,
+          "y": 3.417951,
+          "z": 1.12395
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 28,
+      "pose": {
+        "translation": {
+          "x": 4.574159,
+          "y": 0.6312154,
+          "z": 0.889
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 29,
+      "pose": {
+        "translation": {
+          "x": 0.0136906,
+          "y": 0.6507734,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 30,
+      "pose": {
+        "translation": {
+          "x": 0.0136906,
+          "y": 1.0825734,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 31,
+      "pose": {
+        "translation": {
+          "x": 0.0140462,
+          "y": 3.7301932,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 32,
+      "pose": {
+        "translation": {
+          "x": 0.0140462,
+          "y": 4.1619931999999995,
+          "z": 0.55245
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    }
+  ],
+  "field": {
+    "length": 16.518,
+    "width": 8.043
+  }
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index d9c483a..37bf02d 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -77,13 +77,13 @@ public static final class DeviceIDs {
     public static final int kBackRightModuleEncoder =
         DriveConstants.TunerConstants.BackRight.EncoderId; // 22
 
-    public static final int kLeftTurretFlywheel = 9;
-    public static final int kLeftTurretHood = 10;
-    public static final int kLeftTurretAzimuth = 11;
+    public static final int kLeftTurretFlywheel = 12;
+    public static final int kLeftTurretHood = 13;
+    public static final int kLeftTurretAzimuth = 14;
 
-    public static final int kRightTurretFlywheel = 12;
-    public static final int kRightTurretHood = 13;
-    public static final int kRightTurretAzimuth = 14;
+    public static final int kRightTurretFlywheel = 9;
+    public static final int kRightTurretHood = 10;
+    public static final int kRightTurretAzimuth = 11;
 
     public static final int kLeftGuts = 15;
     public static final int kRightGuts = 16;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index f73eb92..5eff2aa 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -48,7 +48,6 @@
 import frc.robot.subsystems.shooter.hood.HoodIOSim;
 import frc.robot.subsystems.shooter.hood.HoodIOSparkMax;
 import frc.robot.subsystems.shooter.turret.TurretIOSim;
-import frc.robot.subsystems.shooter.turret.TurretIOSparkMax;
 import frc.robot.subsystems.vision.CameraIOLimelight;
 import frc.robot.subsystems.vision.Vision;
 import frc.robot.subsystems.vision.Vision.VisionConsumer;
@@ -82,13 +81,11 @@ public RobotContainer() {
         leftShooter =
             new Shooter(
                 ShooterSide.LEFT,
-                new TurretIOSparkMax(ShooterSide.LEFT),
                 new HoodIOSparkMax(ShooterSide.LEFT),
                 new FlywheelIOTalonFX(ShooterSide.LEFT));
         rightShooter =
             new Shooter(
                 ShooterSide.RIGHT,
-                new TurretIOSparkMax(ShooterSide.RIGHT),
                 new HoodIOSparkMax(ShooterSide.RIGHT),
                 new FlywheelIOTalonFX(ShooterSide.RIGHT));
         leftGuts = new Guts(GutSide.LEFT, new GutsIOSparkMax(DeviceIDs.kLeftGuts));
@@ -181,7 +178,7 @@ public void robotPeriodic() {
   }
 
   public Command getAutonomousCommand() {
-    return leftShooter.zeroTurret().alongWith(rightShooter.zeroTurret());
+    return DriveCommands.feedforwardCharacterization(drive);
     // return Commands.print("No autonomous command configured");
   }
 
diff --git a/src/main/java/frc/robot/control/DriverControls.java b/src/main/java/frc/robot/control/DriverControls.java
index 38073cf..13df135 100644
--- a/src/main/java/frc/robot/control/DriverControls.java
+++ b/src/main/java/frc/robot/control/DriverControls.java
@@ -14,12 +14,11 @@
 import frc.robot.util.AllianceFlipUtil;
 import frc.robot.util.Direction;
 import frc.robot.util.FieldConstants;
-import frc.robot.util.FieldConstants.Hub;
 import org.littletonrobotics.junction.AutoLogOutput;
 
 public class DriverControls implements Configurable {
   @AutoLogOutput(key = "Control/DriverControls/mode")
-  private DriverMode mode = DriverMode.ONE_DRIVER;
+  private DriverMode mode = DriverMode.TWO_DRIVERS;
 
   public enum DriverMode {
     ONE_DRIVER,
@@ -93,19 +92,17 @@ public void configure() {
         .rightBumper()
         .whileTrue(
             rightShooter
-                .setFlywheelVelocity(7000)
-                .alongWith(leftShooter.setFlywheelVelocity(7000)));
-
-    operator.rightBumper().whileTrue(leftGuts.runGutForward());
-    operator.rightBumper().whileTrue(rightGuts.runGutForward());
+                .setFlywheelVelocity(1000)
+                .alongWith(leftShooter.setFlywheelVelocity(-1000))
+                .alongWith(leftGuts.runGutForward(), rightGuts.runGutForward()));
 
     operator
         .dPadUp()
         .whileTrue(
             new StartEndCommand(
                 () -> {
-                  leftShooter.setHoodOpenLoop(0.01);
-                  rightShooter.setHoodOpenLoop(0.01);
+                  leftShooter.setHoodOpenLoop(0.05);
+                  rightShooter.setHoodOpenLoop(0.05);
                 },
                 () -> {
                   leftShooter.setHoodOpenLoop(0);
@@ -117,8 +114,8 @@ public void configure() {
         .whileTrue(
             new StartEndCommand(
                 () -> {
-                  leftShooter.setHoodOpenLoop(-0.01);
-                  rightShooter.setHoodOpenLoop(-0.01);
+                  leftShooter.setHoodOpenLoop(-0.05);
+                  rightShooter.setHoodOpenLoop(-0.05);
                 },
                 () -> {
                   leftShooter.setHoodOpenLoop(0);
diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
index d59493c..05d2e33 100644
--- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
+++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
@@ -35,7 +35,6 @@ public GyroIOPigeon2() {
       pigeon.getConfigurator().apply(new Pigeon2Configuration());
     }
 
-    pigeon.setYaw(180.0);
     yaw.setUpdateFrequency(DriveConstants.kOdometryFrequency);
     yawVelocity.setUpdateFrequency(50.0);
     pigeon.optimizeBusUtilization();
@@ -46,7 +45,7 @@ public GyroIOPigeon2() {
   @Override
   public void updateInputs(GyroIOInputs inputs) {
     inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK);
-    inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
+    inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()).rotateBy(Rotation2d.kPi);
     inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble());
 
     inputs.odometryYawTimestamps =
@@ -61,6 +60,6 @@ public void updateInputs(GyroIOInputs inputs) {
 
   @Override
   public void setYaw(Rotation2d angle) {
-    pigeon.setYaw(angle.getDegrees());
+    pigeon.setYaw(angle.rotateBy(Rotation2d.kPi).getDegrees());
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
index ca2491c..e043163 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -2,7 +2,7 @@
 
 public final class IntakeConstants {
   public static final double kPivotMotorSpeed = 0.5;
-  public static final double kRollerMotorSpeed = -0.5;
+  public static final double kRollerMotorSpeed = -0.3;
   public static final double kSignificantlyFasterRollerMotorSpeed = -0.75;
 
   // Change Gear Ratios later
diff --git a/src/main/java/frc/robot/subsystems/leds/LedConstants.java b/src/main/java/frc/robot/subsystems/leds/LedConstants.java
index 15a2e11..c2a5ab1 100644
--- a/src/main/java/frc/robot/subsystems/leds/LedConstants.java
+++ b/src/main/java/frc/robot/subsystems/leds/LedConstants.java
@@ -3,7 +3,7 @@
 public final class LedConstants {
   public static final int kPort = 1;
 
-  public static final int kFullLength = 10;
+  public static final int kFullLength = 14;
   public static final int kLeftTurretBottomLength = 7;
   public static final int kRightTurretBottomLength = 7;
   public static final int kLeftTurretTopLength = 17;
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index c396220..a379f46 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -23,9 +23,9 @@
 public class Shooter extends SubsystemBase {
   private final ShooterSide side;
 
-  private final Turret turret;
-  private final Hood hood;
-  private final Flywheel flywheel;
+  private Turret turret;
+  private Hood hood;
+  private Flywheel flywheel;
 
   /** Creates a new Shooter. */
   public Shooter(ShooterSide side, TurretIO turretIO, HoodIO hoodIO, FlywheelIO flywheelIO) {
@@ -35,10 +35,19 @@ public Shooter(ShooterSide side, TurretIO turretIO, HoodIO hoodIO, FlywheelIO fl
     this.flywheel = new Flywheel(side, flywheelIO);
   }
 
+  public Shooter(ShooterSide side, HoodIO hoodIO, FlywheelIO flywheelIO) {
+    this.side = side;
+    this.turret = null;
+    this.hood = new Hood(side, hoodIO);
+    this.flywheel = new Flywheel(side, flywheelIO);
+  }
+
   @Override
   public void periodic() {
-    turret.periodic();
     hood.periodic();
+    if (turret != null) {
+      turret.periodic();
+    }
     flywheel.periodic();
   }
 
@@ -90,7 +99,9 @@ public static Command shootBothAtTargetNoTurret(
   public void applyCommand(ShooterCommand cmd) {
     flywheel.setVelocity(cmd.wheelRPM());
     hood.setAngle(cmd.hoodAngle());
-    turret.setPosition(cmd.turretAngle());
+    if (turret != null) {
+      turret.setPosition(cmd.turretAngle());
+    }
   }
 
   public void applyCommandNoRotation(ShooterCommand cmd) {
@@ -98,7 +109,7 @@ public void applyCommandNoRotation(ShooterCommand cmd) {
     hood.setAngle(cmd.hoodAngle());
   }
 
-  public Command shootAtTarget(Supplier targetSupplier) {
+  public Command shootAtTargetRotation(Supplier targetSupplier) {
     return Commands.run(
         () -> {
           ShooterCommand cmd = TrajectoryCalculator.calculate(side, targetSupplier.get());
@@ -112,6 +123,18 @@ public Command shootAtTarget(Supplier targetSupplier) {
         flywheel);
   }
 
+  public Command shootAtTargetNoRotation(Supplier targetSupplier) {
+    return Commands.run(
+        () -> {
+          ShooterCommand cmd = TrajectoryCalculator.calculate(side, targetSupplier.get());
+          flywheel.setVelocity(cmd.wheelRPM());
+          hood.setAngle(cmd.hoodAngle());
+        },
+        this,
+        hood,
+        flywheel);
+  }
+
   public Command trackTarget(Supplier targetSupplier) {
     return turret.trackTarget(targetSupplier);
   }
diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java
index 6ace554..23c659b 100644
--- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java
@@ -21,7 +21,6 @@
 public class FlywheelIOTalonFX implements FlywheelIO {
   private final TalonFX motor;
   private final TalonFXConfiguration motorConfig;
-  private final ShooterSide side;
 
   private final StatusSignal velocitySignal;
   private final StatusSignal accelerationSignal;
@@ -31,7 +30,6 @@ public class FlywheelIOTalonFX implements FlywheelIO {
   private final VelocityVoltage velocityRequest = new VelocityVoltage(0).withSlot(0);
 
   public FlywheelIOTalonFX(ShooterSide side) {
-    this.side = side;
     motor =
         new TalonFX(
             side == ShooterSide.LEFT
@@ -42,15 +40,17 @@ public FlywheelIOTalonFX(ShooterSide side) {
             .withMotorOutput(
                 new MotorOutputConfigs()
                     .withInverted(
-                        side == ShooterSide.LEFT
+                        side == ShooterSide.RIGHT
                             ? InvertedValue.Clockwise_Positive
                             : InvertedValue.CounterClockwise_Positive))
-            .withSlot0(FlywheelConstants.kGains);
-    /**
-     * TODO: Update gains Peiwei, Ben: see the FlywheelConstants.kGains above... thats where the
-     * values are You also might have to check if the inverted values are correct, positive should
-     * spin the right way for shooting (line above that has the withInverted() method)
-     */
+            .withSlot0(FlywheelConstants.kGains)
+            /**
+             * TODO: Update gains Peiwei, Ben: see the FlywheelConstants.kGains above... thats where
+             * the values are You also might have to check if the inverted values are correct,
+             * positive should spin the right way for shooting (line above that has the
+             * withInverted() method)
+             */
+            .withMotorOutput(FlywheelConstants.kOutputConfigs);
     tryUntilOk(5, () -> motor.getConfigurator().apply(motorConfig, 0.25));
 
     velocitySignal = motor.getVelocity();
diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
index c40eb6b..4540cb2 100644
--- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
@@ -39,7 +39,7 @@ public HoodIOSparkMax(ShooterSide side) {
 
     config.idleMode(IdleMode.kCoast);
 
-    config.inverted(side == ShooterSide.LEFT);
+    config.inverted(side == ShooterSide.RIGHT);
 
     config
         .encoder
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
index 79528ec..b97880f 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
@@ -41,7 +41,7 @@ public TurretIOSparkMax(ShooterSide side) {
 
     config.idleMode(IdleMode.kCoast);
     // TODO: Tune
-    config.inverted(side == ShooterSide.LEFT);
+    config.inverted(side == ShooterSide.RIGHT);
     // .smartCurrentLimit(30);
 
     config

From f8e5a496e04c52729f66f83c777a2192a9d053ad Mon Sep 17 00:00:00 2001
From: Sim-City 
Date: Sat, 7 Mar 2026 20:26:03 -0500
Subject: [PATCH 48/61] maxwell made me do it #mm26 #trolling #github

---
 src/main/java/frc/robot/RobotContainer.java   | 15 +++++++++++++-
 .../frc/robot/control/DriverControls.java     | 20 +++++++++++++++----
 .../subsystems/drive/DriveConstants.java      |  3 ++-
 .../robot/subsystems/guts/GutsConstants.java  |  2 +-
 .../subsystems/intake/IntakeConstants.java    |  4 ++--
 .../java/frc/robot/subsystems/leds/Leds.java  |  9 ++++++++-
 .../shooter/TrajectoryCalculator.java         |  6 +++++-
 7 files changed, 48 insertions(+), 11 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 5eff2aa..f7be767 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,6 +4,8 @@
 
 package frc.robot;
 
+import static edu.wpi.first.units.Units.Seconds;
+
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.config.PIDConstants;
@@ -178,7 +180,18 @@ public void robotPeriodic() {
   }
 
   public Command getAutonomousCommand() {
-    return DriveCommands.feedforwardCharacterization(drive);
+    return intake
+        .deploy()
+        .withTimeout(Seconds.of(2))
+        .andThen(
+            rightShooter
+                .setFlywheelVelocity(8500)
+                .alongWith(
+                    leftShooter.setFlywheelVelocity(-8500),
+                    leftGuts.runGutForward(),
+                    rightGuts.runGutForward(),
+                    intake.intake())
+                .withTimeout(10));
     // return Commands.print("No autonomous command configured");
   }
 
diff --git a/src/main/java/frc/robot/control/DriverControls.java b/src/main/java/frc/robot/control/DriverControls.java
index 13df135..fce73a2 100644
--- a/src/main/java/frc/robot/control/DriverControls.java
+++ b/src/main/java/frc/robot/control/DriverControls.java
@@ -92,9 +92,12 @@ public void configure() {
         .rightBumper()
         .whileTrue(
             rightShooter
-                .setFlywheelVelocity(1000)
-                .alongWith(leftShooter.setFlywheelVelocity(-1000))
-                .alongWith(leftGuts.runGutForward(), rightGuts.runGutForward()));
+                .setFlywheelVelocity(8500)
+                .alongWith(leftShooter.setFlywheelVelocity(-8500)));
+
+    operator
+        .rightTrigger()
+        .whileTrue(leftGuts.runGutForward().alongWith(rightGuts.runGutForward()));
 
     operator
         .dPadUp()
@@ -122,10 +125,19 @@ public void configure() {
                   rightShooter.setHoodOpenLoop(0);
                 }));
 
-    operator.leftTrigger().whileTrue(intake.intakeSignificantlyFaster());
+    operator.leftTrigger().whileTrue(intake.outtake());
     operator.aCross().whileTrue(intake.outtake());
     operator.xSquare().whileTrue(intake.deploy());
     operator.yTriangle().whileTrue(intake.retract());
+    operator
+        .bCircle()
+        .whileTrue(
+            rightShooter
+                .setFlywheelVelocity(2000)
+                .alongWith(
+                    leftShooter.setFlywheelVelocity(-2000),
+                    rightGuts.runGutForward(),
+                    leftGuts.runGutForward()));
   }
 
   /*
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
index 4e323c8..9f897fb 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -96,13 +96,14 @@ public class TunerConstants {
     // When using closed-loop control, the drive motor uses the control
     // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
     private static final Slot0Configs driveGains =
-        new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124);
+        new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.3);
 
     // The closed-loop output type to use for the steer motors;
     // This affects the PID/FF gains for the steer motors
     private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage;
     // The closed-loop output type to use for the drive motors;
     // This affects the PID/FF gains for the drive motors
+    
     private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage;
 
     // The type of motor used for the drive motor
diff --git a/src/main/java/frc/robot/subsystems/guts/GutsConstants.java b/src/main/java/frc/robot/subsystems/guts/GutsConstants.java
index 8fd42bc..9b263bb 100644
--- a/src/main/java/frc/robot/subsystems/guts/GutsConstants.java
+++ b/src/main/java/frc/robot/subsystems/guts/GutsConstants.java
@@ -2,7 +2,7 @@
 
 public final class GutsConstants {
 
-  public static final double kGutMotorSpeed = 0.75;
+  public static final double kGutMotorSpeed = 0.6;
   // Change Gear Ratio later
   public static final double kGutMotorGearRatio = 1.0;
 }
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
index e043163..9af2f36 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -1,8 +1,8 @@
 package frc.robot.subsystems.intake;
 
 public final class IntakeConstants {
-  public static final double kPivotMotorSpeed = 0.5;
-  public static final double kRollerMotorSpeed = -0.3;
+  public static final double kPivotMotorSpeed = 0.4;
+  public static final double kRollerMotorSpeed = -0.8;
   public static final double kSignificantlyFasterRollerMotorSpeed = -0.75;
 
   // Change Gear Ratios later
diff --git a/src/main/java/frc/robot/subsystems/leds/Leds.java b/src/main/java/frc/robot/subsystems/leds/Leds.java
index 02c47aa..4b3c005 100644
--- a/src/main/java/frc/robot/subsystems/leds/Leds.java
+++ b/src/main/java/frc/robot/subsystems/leds/Leds.java
@@ -2,6 +2,7 @@
 
 import edu.wpi.first.wpilibj.AddressableLED;
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -67,7 +68,13 @@ private Leds() {
 
   @Override
   public void periodic() {
-    solid(LedSection.ALL, Color.kAqua);
+    if (RobotState.isAutonomous()) {
+      solid(LedSection.ALL, Color.kOrange);
+    } else if (RobotState.isDisabled()) {
+      breath(LedSection.ALL, Color.kRed, Color.kBlack, 3);
+    } else {
+      solid(LedSection.ALL, Color.kAqua);
+    }
     // solid(LedSection.TOP_LEFT_TURRET, Color.kLimeGreen);
     // solid(LedSection.BOTTOM_LEFT_TURRET, Color.kYellow);
     // solid(LedSection.BOTTOM_RIGHT_TURRET, Color.kSkyBlue);
diff --git a/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java
index 2437db2..2d91069 100644
--- a/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java
+++ b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java
@@ -26,7 +26,7 @@ public class TrajectoryCalculator {
   static {
     shooterTable.put(1.5, new TrajectoryParams(2800.0, 35.0, 0.38));
     shooterTable.put(2.0, new TrajectoryParams(3100.0, 38.0, 0.45));
-    shooterTable.put(2.5, new TrajectoryParams(3400.0, 42.0, 0.52));
+    shooterTable.put(2.6289, new TrajectoryParams(5000.0, 42.0, 0.52));
     shooterTable.put(3.0, new TrajectoryParams(3650.0, 46.0, 0.60));
     shooterTable.put(3.5, new TrajectoryParams(3900.0, 50.0, 0.68));
     shooterTable.put(4.0, new TrajectoryParams(4100.0, 54.0, 0.76));
@@ -45,6 +45,10 @@ public static ShooterCommand calculate(ShooterSide side, Translation2d targetLoc
     return calculateWithState(side, targetLocation, state);
   }
 
+  public static double calculateRPM(Translation2d targetLocation, Pose2d robotPose) {
+    return shooterTable.get(targetLocation.getDistance(robotPose.getTranslation())).wheelRPM;
+  }
+
   /**
    * Calculate shooter commands for both shooters efficiently. Use this when both shooters need
    * calculation - avoids duplicate state queries.

From f1c72323682e4d0931777b8159715332015aab7c Mon Sep 17 00:00:00 2001
From: Maxwell Morgan 
Date: Thu, 12 Mar 2026 16:59:08 -0400
Subject: [PATCH 49/61] Update turret pid

---
 .../apriltags/2026-rebuilt-andymark.json      | 584 ------------------
 src/main/java/frc/robot/RobotContainer.java   | 269 +++-----
 src/main/java/frc/robot/RobotState.java       |   6 +-
 .../frc/robot/subsystems/drive/Drive.java     |  23 +-
 .../subsystems/drive/DriveConstants.java      |   2 +-
 .../frc/robot/subsystems/drive/GyroIO.java    |   2 -
 .../robot/subsystems/drive/GyroIONavX.java    |  18 +-
 .../robot/subsystems/drive/GyroIOPigeon2.java |  20 +-
 .../subsystems/shooter/ShooterConstants.java  |   2 +-
 .../subsystems/shooter/turret/Turret.java     |  59 +-
 .../shooter/turret/TurretIOSparkMax.java      |  32 +-
 .../frc/robot/subsystems/vision/Vision.java   |   4 +
 12 files changed, 150 insertions(+), 871 deletions(-)
 delete mode 100644 src/main/deploy/apriltags/2026-rebuilt-andymark.json

diff --git a/src/main/deploy/apriltags/2026-rebuilt-andymark.json b/src/main/deploy/apriltags/2026-rebuilt-andymark.json
deleted file mode 100644
index ecc0390..0000000
--- a/src/main/deploy/apriltags/2026-rebuilt-andymark.json
+++ /dev/null
@@ -1,584 +0,0 @@
-{
-  "tags": [
-    {
-      "ID": 1,
-      "pose": {
-        "translation": {
-          "x": 11.863959,
-          "y": 7.411491399999999,
-          "z": 0.889
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 6.123233995736766e-17,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 1.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 2,
-      "pose": {
-        "translation": {
-          "x": 11.9013986,
-          "y": 4.6247558,
-          "z": 1.12395
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 0.7071067811865476,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 0.7071067811865476
-          }
-        }
-      }
-    },
-    {
-      "ID": 3,
-      "pose": {
-        "translation": {
-          "x": 11.2978438,
-          "y": 4.3769534,
-          "z": 1.12395
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 6.123233995736766e-17,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 1.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 4,
-      "pose": {
-        "translation": {
-          "x": 11.2978438,
-          "y": 4.0213534,
-          "z": 1.12395
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 6.123233995736766e-17,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 1.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 5,
-      "pose": {
-        "translation": {
-          "x": 11.9013986,
-          "y": 3.417951,
-          "z": 1.12395
-        },
-        "rotation": {
-          "quaternion": {
-            "W": -0.7071067811865475,
-            "X": -0.0,
-            "Y": 0.0,
-            "Z": 0.7071067811865476
-          }
-        }
-      }
-    },
-    {
-      "ID": 6,
-      "pose": {
-        "translation": {
-          "x": 11.863959,
-          "y": 0.6312154,
-          "z": 0.889
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 6.123233995736766e-17,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 1.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 7,
-      "pose": {
-        "translation": {
-          "x": 11.9388636,
-          "y": 0.6312154,
-          "z": 0.889
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 1.0,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 0.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 8,
-      "pose": {
-        "translation": {
-          "x": 12.2569986,
-          "y": 3.417951,
-          "z": 1.12395
-        },
-        "rotation": {
-          "quaternion": {
-            "W": -0.7071067811865475,
-            "X": -0.0,
-            "Y": 0.0,
-            "Z": 0.7071067811865476
-          }
-        }
-      }
-    },
-    {
-      "ID": 9,
-      "pose": {
-        "translation": {
-          "x": 12.5051566,
-          "y": 3.6657534,
-          "z": 1.12395
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 1.0,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 0.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 10,
-      "pose": {
-        "translation": {
-          "x": 12.5051566,
-          "y": 4.0213534,
-          "z": 1.12395
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 1.0,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 0.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 11,
-      "pose": {
-        "translation": {
-          "x": 12.2569986,
-          "y": 4.6247558,
-          "z": 1.12395
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 0.7071067811865476,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 0.7071067811865476
-          }
-        }
-      }
-    },
-    {
-      "ID": 12,
-      "pose": {
-        "translation": {
-          "x": 11.9388636,
-          "y": 7.411491399999999,
-          "z": 0.889
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 1.0,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 0.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 13,
-      "pose": {
-        "translation": {
-          "x": 16.499332,
-          "y": 7.391907999999999,
-          "z": 0.55245
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 6.123233995736766e-17,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 1.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 14,
-      "pose": {
-        "translation": {
-          "x": 16.499332,
-          "y": 6.960107999999999,
-          "z": 0.55245
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 6.123233995736766e-17,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 1.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 15,
-      "pose": {
-        "translation": {
-          "x": 16.4989764,
-          "y": 4.3124882,
-          "z": 0.55245
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 6.123233995736766e-17,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 1.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 16,
-      "pose": {
-        "translation": {
-          "x": 16.4989764,
-          "y": 3.8806881999999994,
-          "z": 0.55245
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 6.123233995736766e-17,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 1.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 17,
-      "pose": {
-        "translation": {
-          "x": 4.6490636,
-          "y": 0.6312154,
-          "z": 0.889
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 1.0,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 0.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 18,
-      "pose": {
-        "translation": {
-          "x": 4.6115986,
-          "y": 3.417951,
-          "z": 1.12395
-        },
-        "rotation": {
-          "quaternion": {
-            "W": -0.7071067811865475,
-            "X": -0.0,
-            "Y": 0.0,
-            "Z": 0.7071067811865476
-          }
-        }
-      }
-    },
-    {
-      "ID": 19,
-      "pose": {
-        "translation": {
-          "x": 5.2151534,
-          "y": 3.6657534,
-          "z": 1.12395
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 1.0,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 0.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 20,
-      "pose": {
-        "translation": {
-          "x": 5.2151534,
-          "y": 4.0213534,
-          "z": 1.12395
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 1.0,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 0.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 21,
-      "pose": {
-        "translation": {
-          "x": 4.6115986,
-          "y": 4.6247558,
-          "z": 1.12395
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 0.7071067811865476,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 0.7071067811865476
-          }
-        }
-      }
-    },
-    {
-      "ID": 22,
-      "pose": {
-        "translation": {
-          "x": 4.6490636,
-          "y": 7.411491399999999,
-          "z": 0.889
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 1.0,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 0.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 23,
-      "pose": {
-        "translation": {
-          "x": 4.574159,
-          "y": 7.411491399999999,
-          "z": 0.889
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 6.123233995736766e-17,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 1.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 24,
-      "pose": {
-        "translation": {
-          "x": 4.2559986,
-          "y": 4.6247558,
-          "z": 1.12395
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 0.7071067811865476,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 0.7071067811865476
-          }
-        }
-      }
-    },
-    {
-      "ID": 25,
-      "pose": {
-        "translation": {
-          "x": 4.007866,
-          "y": 4.3769534,
-          "z": 1.12395
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 6.123233995736766e-17,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 1.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 26,
-      "pose": {
-        "translation": {
-          "x": 4.007866,
-          "y": 4.0213534,
-          "z": 1.12395
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 6.123233995736766e-17,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 1.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 27,
-      "pose": {
-        "translation": {
-          "x": 4.2559986,
-          "y": 3.417951,
-          "z": 1.12395
-        },
-        "rotation": {
-          "quaternion": {
-            "W": -0.7071067811865475,
-            "X": -0.0,
-            "Y": 0.0,
-            "Z": 0.7071067811865476
-          }
-        }
-      }
-    },
-    {
-      "ID": 28,
-      "pose": {
-        "translation": {
-          "x": 4.574159,
-          "y": 0.6312154,
-          "z": 0.889
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 6.123233995736766e-17,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 1.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 29,
-      "pose": {
-        "translation": {
-          "x": 0.0136906,
-          "y": 0.6507734,
-          "z": 0.55245
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 1.0,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 0.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 30,
-      "pose": {
-        "translation": {
-          "x": 0.0136906,
-          "y": 1.0825734,
-          "z": 0.55245
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 1.0,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 0.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 31,
-      "pose": {
-        "translation": {
-          "x": 0.0140462,
-          "y": 3.7301932,
-          "z": 0.55245
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 1.0,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 0.0
-          }
-        }
-      }
-    },
-    {
-      "ID": 32,
-      "pose": {
-        "translation": {
-          "x": 0.0140462,
-          "y": 4.1619931999999995,
-          "z": 0.55245
-        },
-        "rotation": {
-          "quaternion": {
-            "W": 1.0,
-            "X": 0.0,
-            "Y": 0.0,
-            "Z": 0.0
-          }
-        }
-      }
-    }
-  ],
-  "field": {
-    "length": 16.518,
-    "width": 8.043
-  }
-}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index f7be767..1b0735a 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,230 +4,99 @@
 
 package frc.robot;
 
-import static edu.wpi.first.units.Units.Seconds;
-
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.auto.NamedCommands;
-import com.pathplanner.lib.config.PIDConstants;
-import com.pathplanner.lib.controllers.PPHolonomicDriveController;
-import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.numbers.N1;
-import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+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.InstantCommand;
-import frc.robot.Constants.DeviceIDs;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import frc.robot.RobotState.OdometryObservation;
-import frc.robot.RobotState.VisionMeasurement;
-import frc.robot.commands.DriveCommands;
-import frc.robot.control.Configurable;
-import frc.robot.control.DefaultControls;
 import frc.robot.control.DriverController;
-import frc.robot.control.DriverControls;
 import frc.robot.subsystems.drive.Drive;
-import frc.robot.subsystems.drive.DriveConstants.TunerConstants;
-import frc.robot.subsystems.drive.GyroIO;
 import frc.robot.subsystems.drive.GyroIOPigeon2;
 import frc.robot.subsystems.drive.ModuleIO;
-import frc.robot.subsystems.drive.ModuleIOSim;
-import frc.robot.subsystems.drive.ModuleIOTalonFX;
-import frc.robot.subsystems.guts.Guts;
-import frc.robot.subsystems.guts.Guts.GutSide;
-import frc.robot.subsystems.guts.GutsIOSim;
-import frc.robot.subsystems.guts.GutsIOSparkMax;
-import frc.robot.subsystems.intake.Intake;
-import frc.robot.subsystems.intake.IntakeIOHardware;
-import frc.robot.subsystems.intake.IntakeIOSim;
-import frc.robot.subsystems.leds.Leds;
-import frc.robot.subsystems.leds.Leds.LedSection;
-import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.Shooter.ShooterSide;
-import frc.robot.subsystems.shooter.flywheel.FlywheelIOSim;
-import frc.robot.subsystems.shooter.flywheel.FlywheelIOTalonFX;
-import frc.robot.subsystems.shooter.hood.HoodIOSim;
-import frc.robot.subsystems.shooter.hood.HoodIOSparkMax;
-import frc.robot.subsystems.shooter.turret.TurretIOSim;
-import frc.robot.subsystems.vision.CameraIOLimelight;
+import frc.robot.subsystems.shooter.turret.Turret;
+import frc.robot.subsystems.shooter.turret.TurretIOSparkMax;
 import frc.robot.subsystems.vision.Vision;
-import frc.robot.subsystems.vision.Vision.VisionConsumer;
-import frc.robot.util.AllianceFlipUtil;
-import frc.robot.util.FieldConstants;
-import java.util.List;
+import java.util.function.Supplier;
 
 public class RobotContainer {
   private final DriverController driver = new DriverController.XboxDriverController(0);
   private final DriverController operator = new DriverController.XboxDriverController(1);
 
-  private Drive drive;
-  private Shooter leftShooter;
-  private Shooter rightShooter;
-  private Guts leftGuts;
-  private Guts rightGuts;
-  private Intake intake;
+  private Turret turret;
   private Vision vision;
+  private Drive drive;
 
-  public RobotContainer() {
-    switch (Constants.kCurrentMode) {
-      case REAL:
-        drive =
-            new Drive(
-                new GyroIOPigeon2(),
-                new ModuleIOTalonFX(TunerConstants.FrontLeft),
-                new ModuleIOTalonFX(TunerConstants.FrontRight),
-                new ModuleIOTalonFX(TunerConstants.BackLeft),
-                new ModuleIOTalonFX(TunerConstants.BackRight));
-        // vision = new Vision(null, null);
-        leftShooter =
-            new Shooter(
-                ShooterSide.LEFT,
-                new HoodIOSparkMax(ShooterSide.LEFT),
-                new FlywheelIOTalonFX(ShooterSide.LEFT));
-        rightShooter =
-            new Shooter(
-                ShooterSide.RIGHT,
-                new HoodIOSparkMax(ShooterSide.RIGHT),
-                new FlywheelIOTalonFX(ShooterSide.RIGHT));
-        leftGuts = new Guts(GutSide.LEFT, new GutsIOSparkMax(DeviceIDs.kLeftGuts));
-        rightGuts = new Guts(GutSide.RIGHT, new GutsIOSparkMax(DeviceIDs.kRightGuts));
-        intake = new Intake(new IntakeIOHardware());
-        vision =
-            new Vision(
-                new VisionConsumer() {
-                  @Override
-                  public void accept(
-                      Pose2d visionRobotPoseMeters,
-                      double timestampSeconds,
-                      Matrix visionMeasurementStdDevs) {
-                    RobotState.getInstance()
-                        .addVisionMeasurement(
-                            new VisionMeasurement(
-                                timestampSeconds, visionRobotPoseMeters, visionMeasurementStdDevs));
-                  }
-                },
-                new CameraIOLimelight(
-                    "limelight-front", () -> RobotState.getInstance().getRotation()),
-                new CameraIOLimelight(
-                    "limelight-left", () -> RobotState.getInstance().getRotation()));
-        break;
-      case SIM:
-        drive =
-            new Drive(
-                new GyroIO() {},
-                new ModuleIOSim(TunerConstants.FrontLeft),
-                new ModuleIOSim(TunerConstants.FrontRight),
-                new ModuleIOSim(TunerConstants.BackLeft),
-                new ModuleIOSim(TunerConstants.BackRight));
-        leftShooter =
-            new Shooter(ShooterSide.LEFT, new TurretIOSim(), new HoodIOSim(), new FlywheelIOSim());
-        rightShooter =
-            new Shooter(ShooterSide.RIGHT, new TurretIOSim(), new HoodIOSim(), new FlywheelIOSim());
-        // vision = new Vision(null, null);
-        drive =
-            new Drive(
-                new GyroIO() {},
-                new ModuleIOSim(TunerConstants.FrontLeft),
-                new ModuleIOSim(TunerConstants.FrontRight),
-                new ModuleIOSim(TunerConstants.BackLeft),
-                new ModuleIOSim(TunerConstants.BackRight));
-        // vision = new Vision(null, null);
-        leftGuts = new Guts(GutSide.LEFT, new GutsIOSim());
-        rightGuts = new Guts(GutSide.RIGHT, new GutsIOSim());
-        intake = new Intake(new IntakeIOSim());
-
-        break;
-      case REPLAY:
-      default:
-        drive =
-            new Drive(
-                new GyroIO() {},
-                new ModuleIO() {},
-                new ModuleIO() {},
-                new ModuleIO() {},
-                new ModuleIO() {});
-        leftShooter = new Shooter(null, null, null, null);
-        rightShooter = new Shooter(null, null, null, null);
-        // vision = new Vision(null, new CameraIO[] {});
-        break;
-    }
-    Leds.getInstance().solid(LedSection.ALL, Color.kCyan);
-
-    configureBindings();
-  }
+  private Field2d field2d = new Field2d();
 
-  private void configureBindings() {
-    List.of(
-            new DefaultControls(driver, operator, drive, leftShooter, rightShooter),
-            new DriverControls(
-                driver, operator, drive, leftShooter, rightShooter, leftGuts, rightGuts, intake)
-            // // TODO: Implement ZoneControls
-            // // ,new ZoneControls()
-            )
-        .forEach(Configurable::configure);
+  public RobotContainer() {
+    turret = new Turret(ShooterSide.LEFT, new TurretIOSparkMax(ShooterSide.LEFT));
+    Supplier robotRotationSupplier = () -> RobotState.getInstance().getRotation();
+    // vision =
+    //     new Vision(
+    //         new VisionConsumer() {
+    //           public void accept(
+    //               Pose2d visionRobotPoseMeters,
+    //               double timestampSeconds,
+    //               edu.wpi.first.math.Matrix visionMeasurementStdDevs) {
+
+    //             RobotState.getInstance()
+    //                 .addVisionMeasurement(
+    //                     new VisionMeasurement(
+    //                         timestampSeconds, visionRobotPoseMeters, visionMeasurementStdDevs));
+    //           }
+    //           ;
+    //         },
+    //         new CameraIOLimelight("limelight-front", robotRotationSupplier),
+    //         new CameraIOLimelight("limelight", robotRotationSupplier));
+    drive =
+        new Drive(
+            new GyroIOPigeon2(),
+            new ModuleIO() {},
+            new ModuleIO() {},
+            new ModuleIO() {},
+            new ModuleIO() {});
+
+    turret.setDefaultCommand(
+        new RunCommand(() -> turret.setPosition(RobotState.getInstance().getRotation()), turret));
+
+    driver
+        .rightBumper()
+        .whileTrue(
+            Commands.runEnd(() -> turret.setOpenLoop(0.1), () -> turret.setOpenLoop(0), turret));
+
+    driver
+        .leftBumper()
+        .whileTrue(
+            Commands.runEnd(() -> turret.setOpenLoop(-0.1), () -> turret.setOpenLoop(0), turret));
   }
 
   public void robotPeriodic() {
-    OdometryObservation obs =
-        new OdometryObservation(
-            Timer.getTimestamp(), drive.getModulePositions(), drive.getRawGyroRotation());
-    RobotState.getInstance().addOdometryObservation(obs);
-    RobotState.getInstance().setRobotVelocity(drive.getChassisSpeeds());
-    // System.out.println(RobotState.getInstance().getEstimatedPose().getX());
-    // System.out.println(RobotState.getInstance().getRobotVelocity().vxMetersPerSecond);
-    // System.out.println(driver.getLeftX());
+    RobotState.getInstance()
+        .addOdometryObservation(
+            new OdometryObservation(
+                Timer.getTimestamp(),
+                new SwerveModulePosition[] {
+                  new SwerveModulePosition(
+                      Math.random(), new Rotation2d(Math.random(), Math.random())),
+                  new SwerveModulePosition(
+                      Math.random(), new Rotation2d(Math.random(), Math.random())),
+                  new SwerveModulePosition(
+                      Math.random(), new Rotation2d(Math.random(), Math.random())),
+                  new SwerveModulePosition(
+                      Math.random(), new Rotation2d(Math.random(), Math.random()))
+                },
+                drive.getRawGyroRotation()));
   }
 
   public Command getAutonomousCommand() {
-    return intake
-        .deploy()
-        .withTimeout(Seconds.of(2))
-        .andThen(
-            rightShooter
-                .setFlywheelVelocity(8500)
-                .alongWith(
-                    leftShooter.setFlywheelVelocity(-8500),
-                    leftGuts.runGutForward(),
-                    rightGuts.runGutForward(),
-                    intake.intake())
-                .withTimeout(10));
-    // return Commands.print("No autonomous command configured");
+    return Commands.print("No autonomous command configured");
   }
 
-  public void configurePathPlanner() {
-    AutoBuilder.configure(
-        () -> RobotState.getInstance().getEstimatedPose(),
-        (pose) -> RobotState.getInstance().setPose(pose),
-        () -> RobotState.getInstance().getRobotVelocity(),
-        (speeds, feedforwards) -> drive.runVelocity(speeds),
-        new PPHolonomicDriveController(new PIDConstants(5, 0, 0), new PIDConstants(0, 0, 0)),
-        Constants.kRobotConfig,
-        AllianceFlipUtil::shouldFlip,
-        drive);
-
-    /** PATHPLANNER COMMANDS */
-    NamedCommands.registerCommand(
-        "resetGyro",
-        new InstantCommand(() -> RobotState.getInstance().resetRotation(Rotation2d.kZero)));
-
-    NamedCommands.registerCommand(
-        "Shoot Both At Hub",
-        Shooter.shootBothAtTargetNoTurret(
-                leftShooter,
-                rightShooter,
-                () -> AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d()))
-            .alongWith(leftGuts.runGutForward(), rightGuts.runGutForward()));
-
-    NamedCommands.registerCommand("Intake/Index Fuel", intake.intake());
-    NamedCommands.registerCommand("Deploy Intake", intake.intake());
-    NamedCommands.registerCommand("Retract Intake", intake.intake());
-
-    NamedCommands.registerCommand(
-        "Turn Robot To Hub",
-        DriveCommands.turnToPoint(
-            drive,
-            () -> RobotState.getInstance().getEstimatedPose(),
-            () -> AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d())));
-  }
+  public void configurePathPlanner() {}
 }
diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java
index 4420459..90a8bd4 100644
--- a/src/main/java/frc/robot/RobotState.java
+++ b/src/main/java/frc/robot/RobotState.java
@@ -57,6 +57,9 @@ public void addOdometryObservation(OdometryObservation observation) {
         observation.timestamp(), observation.gyroAngle(), observation.modulePositions());
 
     Logger.recordOutput("RobotState/EstimatedPose", poseEstimator.getEstimatedPosition());
+    Logger.recordOutput(
+        "RobotState/EstimatedRotation",
+        poseEstimator.getEstimatedPosition().getRotation().getDegrees());
   }
 
   /**
@@ -65,8 +68,7 @@ public void addOdometryObservation(OdometryObservation observation) {
    * @param measurement A {@link VisionMeasurement} object representing the vision pose estimate.
    */
   public void addVisionMeasurement(VisionMeasurement measurement) {
-    poseEstimator.addVisionMeasurement(
-        measurement.visionPose(), measurement.timestamp(), measurement.stdDevs());
+    poseEstimator.addVisionMeasurement(measurement.visionPose(), measurement.timestamp());
 
     Logger.recordOutput("RobotState/EstimatedPose", poseEstimator.getEstimatedPosition());
   }
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
index 490e2bf..264eb2a 100644
--- a/src/main/java/frc/robot/subsystems/drive/Drive.java
+++ b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -14,7 +14,6 @@
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -44,7 +43,7 @@ public class Drive extends SubsystemBase {
 
   private final SwerveDriveKinematics kinematics = DriveConstants.kSwerveKinematics;
 
-  private Rotation2d rawGyroRotation = Rotation2d.kZero;
+  private Rotation2d rawGyroRotation;
   private SwerveModulePosition[] lastModulePositions = // For delta tracking
       new SwerveModulePosition[] {
         new SwerveModulePosition(),
@@ -70,6 +69,7 @@ public Drive(
 
     // Start odometry thread
     PhoenixOdometryThread.getInstance().start();
+    rawGyroRotation = Rotation2d.kZero;
 
     // Configure SysId
     sysId =
@@ -88,6 +88,7 @@ public void periodic() {
     odometryLock.lock(); // Prevents odometry updates while reading data
     gyroIO.updateInputs(gyroInputs);
     Logger.processInputs("Drive/Gyro", gyroInputs);
+    rawGyroRotation = gyroInputs.yawPosition;
     for (var module : modules) {
       module.periodic();
     }
@@ -126,15 +127,15 @@ public void periodic() {
                 modulePositions[moduleIndex].distanceMeters, modulePositions[moduleIndex].angle);
       }
 
-      // Update gyro angle
-      if (gyroInputs.connected) {
-        // Use the real gyro angle
-        rawGyroRotation = gyroInputs.odometryYawPositions[i];
-      } else {
-        // Use the angle delta from the kinematics and module deltas
-        Twist2d twist = kinematics.toTwist2d(moduleDeltas);
-        rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta));
-      }
+      // // Update gyro angle
+      // if (gyroInputs.connected) {
+      //   // Use the real gyro angle
+      //   rawGyroRotation = gyroInputs.yawPosition;
+      // } else {
+      //   // Use the angle delta from the kinematics and module deltas
+      //   Twist2d twist = kinematics.toTwist2d(moduleDeltas);
+      //   rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta));
+      // }
 
       // Apply update (doesn't work)
       // RobotState.getInstance()
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
index 9f897fb..c973310 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -103,7 +103,7 @@ public class TunerConstants {
     private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage;
     // The closed-loop output type to use for the drive motors;
     // This affects the PID/FF gains for the drive motors
-    
+
     private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage;
 
     // The type of motor used for the drive motor
diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java
index 910155c..c671784 100644
--- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java
+++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java
@@ -16,8 +16,6 @@ public static class GyroIOInputs {
     public boolean connected = false;
     public Rotation2d yawPosition = Rotation2d.kZero;
     public double yawVelocityRadPerSec = 0.0;
-    public double[] odometryYawTimestamps = new double[] {};
-    public Rotation2d[] odometryYawPositions = new Rotation2d[] {};
   }
 
   public default void updateInputs(GyroIOInputs inputs) {}
diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java
index 55d008c..fdeb75d 100644
--- a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java
+++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java
@@ -11,18 +11,17 @@
 import com.studica.frc.AHRS.NavXComType;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
-import java.util.Queue;
 
 /** IO implementation for NavX. */
 public class GyroIONavX implements GyroIO {
   private final AHRS navX =
       new AHRS(NavXComType.kMXP_SPI, (byte) DriveConstants.kOdometryFrequency);
-  private final Queue yawPositionQueue;
-  private final Queue yawTimestampQueue;
+  // private final Queue yawPositionQueue;
+  // private final Queue yawTimestampQueue;
 
   public GyroIONavX() {
-    yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
-    yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(navX::getYaw);
+    // yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
+    // yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(navX::getYaw);
   }
 
   @Override
@@ -30,15 +29,6 @@ public void updateInputs(GyroIOInputs inputs) {
     inputs.connected = navX.isConnected();
     inputs.yawPosition = Rotation2d.fromDegrees(-navX.getYaw());
     inputs.yawVelocityRadPerSec = Units.degreesToRadians(-navX.getRawGyroZ());
-
-    inputs.odometryYawTimestamps =
-        yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray();
-    inputs.odometryYawPositions =
-        yawPositionQueue.stream()
-            .map((Double value) -> Rotation2d.fromDegrees(-value))
-            .toArray(Rotation2d[]::new);
-    yawTimestampQueue.clear();
-    yawPositionQueue.clear();
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
index 05d2e33..981b839 100644
--- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
+++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
@@ -35,6 +35,7 @@ public GyroIOPigeon2() {
       pigeon.getConfigurator().apply(new Pigeon2Configuration());
     }
 
+    pigeon.setYaw(0);
     yaw.setUpdateFrequency(DriveConstants.kOdometryFrequency);
     yawVelocity.setUpdateFrequency(50.0);
     pigeon.optimizeBusUtilization();
@@ -45,17 +46,16 @@ public GyroIOPigeon2() {
   @Override
   public void updateInputs(GyroIOInputs inputs) {
     inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK);
-    inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()).rotateBy(Rotation2d.kPi);
+    inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
     inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble());
-
-    inputs.odometryYawTimestamps =
-        yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray();
-    inputs.odometryYawPositions =
-        yawPositionQueue.stream()
-            .map((Double value) -> Rotation2d.fromDegrees(value))
-            .toArray(Rotation2d[]::new);
-    yawTimestampQueue.clear();
-    yawPositionQueue.clear();
+    // inputs.odometryYawTimestamps =
+    //     yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray();
+    // inputs.odometryYawPositions =
+    //     yawPositionQueue.stream()
+    //         .map((Double value) -> Rotation2d.fromDegrees(value))
+    //         .toArray(Rotation2d[]::new);
+    // yawTimestampQueue.clear();
+    // yawPositionQueue.clear();
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index a2d8b07..fc8f7e7 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -19,7 +19,7 @@ public static final class TurretConstants {
     public static final double kGearRatio = 10 / 1; // Motor / Turret
     public static final double kMinTurretAngleRad = Units.degreesToRadians(-90);
     public static final double kMaxTurretAngleRad = Units.degreesToRadians(90);
-    public static final double kAngleTolerance = Units.degreesToRadians(2);
+    public static final double kAngleTolerance = Units.degreesToRadians(0.5);
 
     public static final double kLeftMotorId = 12;
     public static final double kRightMotorId = 13;
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
index 1bbf6aa..b6cf435 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
@@ -34,7 +34,7 @@ public class Turret extends FullSubsystem {
   private boolean atGoal = false;
   private Debouncer atGoalDebouncer = new Debouncer(0.1, DebounceType.kFalling);
 
-  private boolean isZeroed = false;
+  private boolean isZeroed = true;
 
   /** Creates a new Turret. */
   public Turret(ShooterSide side, TurretIO io) {
@@ -56,45 +56,49 @@ public void periodic() {
     } else if (side == ShooterSide.RIGHT) {
       RobotVisualizer.getInstance().setRightTurretAngle(Rotation2d.fromRadians(inputs.positionRad));
     }
-
-    Logger.recordOutput(("Turret/" + side.getName() + "/TargetAngle"), targetAngle);
   }
 
   @Override
   public void periodicAfterScheduler() {
     io.applyOutputs(outputs);
+    Logger.recordOutput("Turret/Mode", outputs.mode.toString());
+    Logger.recordOutput(("Turret/" + side.getName() + "/TargetAngle"), targetAngle);
+    Logger.recordOutput(
+        ("Turret/" + side.getName() + "/TargetAngleDegrees"), targetAngle.getDegrees());
+    Logger.recordOutput(
+        ("Turret/" + side.getName() + "/TargetOffsetDegrees"),
+        targetAngle.minus(Rotation2d.fromRadians(inputs.positionRad)).getDegrees());
   }
 
   public Command trackTarget(Supplier targetSupplier) {
 
     return Commands.run(
-        () -> {
-          Translation2d target = targetSupplier.get();
-          Pose2d robotPose = RobotState.getInstance().getEstimatedPose();
+            () -> {
+              Translation2d target = targetSupplier.get();
+              Pose2d robotPose = RobotState.getInstance().getEstimatedPose();
 
-          Translation2d turretOffset =
-              (this.side == ShooterSide.LEFT
-                  ? TurretConstants.kRobotToLeftTurret.getTranslation().toTranslation2d()
-                  : TurretConstants.kRobotToRightTurret.getTranslation().toTranslation2d());
+              Translation2d turretOffset = Translation2d.kZero;
 
-          // Turret position in field coordinates
-          Translation2d turretFieldPos =
-              robotPose.getTranslation().plus(turretOffset.rotateBy(robotPose.getRotation()));
+              // Turret position in field coordinates
+              Translation2d turretFieldPos =
+                  robotPose.getTranslation().plus(turretOffset.rotateBy(robotPose.getRotation()));
 
-          // Vector from turret -> target (field frame)
-          Translation2d deltaField = target.minus(turretFieldPos);
+              // Vector from turret -> target (field frame)
+              Translation2d deltaField = target.minus(turretFieldPos);
+              Logger.recordOutput("Turret Target Distance", deltaField.getNorm());
 
-          // Convert to robot frame
-          Translation2d deltaRobot = deltaField.rotateBy(robotPose.getRotation().unaryMinus());
+              // Convert to robot frame
+              Translation2d deltaRobot = deltaField.rotateBy(robotPose.getRotation().unaryMinus());
 
-          // Angle turret should point (robot-relative)
-          Rotation2d targetAngle = new Rotation2d(Math.atan2(deltaRobot.getY(), deltaRobot.getX()));
+              // Angle turret should point (robot-relative)
+              Rotation2d targetAngle =
+                  Rotation2d.fromRadians(Math.atan2(deltaRobot.getY(), deltaRobot.getX()));
 
-          this.targetAngle = targetAngle;
-
-          setPosition(targetAngle);
-        },
-        this);
+              this.targetAngle = targetAngle;
+              setPosition(targetAngle);
+            },
+            this)
+        .until(() -> this.atGoal);
   }
 
   public Command zero() {
@@ -113,13 +117,6 @@ public void setPosition(Rotation2d position) {
 
     targetAngle = position;
 
-    position =
-        Rotation2d.fromRadians(
-            MathUtil.clamp(
-                position.getRadians(),
-                TurretConstants.kMinTurretAngleRad,
-                TurretConstants.kMaxTurretAngleRad));
-
     outputs.mode = TurretIOOutputMode.CLOSED_LOOP;
     outputs.closedLoopTarget = position;
 
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
index b97880f..330b3af 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
@@ -7,6 +7,7 @@
 import com.revrobotics.PersistMode;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.ResetMode;
+import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.FeedbackSensor;
 import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkClosedLoopController;
@@ -37,6 +38,8 @@ public TurretIOSparkMax(ShooterSide side) {
     encoder = motor.getEncoder();
     motorController = motor.getClosedLoopController();
 
+    encoder.setPosition(0);
+
     SparkMaxConfig config = new SparkMaxConfig();
 
     config.idleMode(IdleMode.kCoast);
@@ -50,16 +53,15 @@ public TurretIOSparkMax(ShooterSide side) {
             2 * Math.PI / TurretConstants.kGearRatio) // No absolute encoder...
         .velocityConversionFactor(2 * Math.PI / TurretConstants.kGearRatio / 60.0);
 
-    config.closedLoop.positionWrappingEnabled(false).feedbackSensor(FeedbackSensor.kPrimaryEncoder);
+    config.closedLoop.positionWrappingEnabled(true).feedbackSensor(FeedbackSensor.kPrimaryEncoder);
 
-    config
-        .softLimit
-        .reverseSoftLimitEnabled(true)
-        .forwardSoftLimitEnabled(true)
-        .reverseSoftLimit(TurretConstants.kMinTurretAngleRad)
-        .forwardSoftLimit(TurretConstants.kMaxTurretAngleRad);
+    config.softLimit.reverseSoftLimitEnabled(false).forwardSoftLimitEnabled(false);
 
-    config.closedLoop.feedForward.kS(0);
+    config.closedLoop.feedForward.kS(0.025 * 12);
+    config.closedLoop.p(0.7);
+    config.closedLoop.d(0.5);
+    config.closedLoop.allowedClosedLoopError(
+        TurretConstants.kAngleTolerance, ClosedLoopSlot.kSlot0);
 
     tryUntilOk(
         motor,
@@ -87,16 +89,16 @@ public void updateInputs(TurretIOInputs inputs) {
   public void applyOutputs(TurretIOOutputs outputs) {
     switch (outputs.mode) {
       case CLOSED_LOOP -> {
-        double clampedPosition =
-            MathUtil.clamp(
-                outputs.closedLoopTarget.getRadians(),
-                TurretConstants.kMinTurretAngleRad,
-                TurretConstants.kMaxTurretAngleRad);
+        // double clampedPosition =
+        //     MathUtil.clamp(
+        //         outputs.closedLoopTarget.getRadians(),
+        //         TurretConstants.kMinTurretAngleRad,
+        //         TurretConstants.kMaxTurretAngleRad);
 
-        motorController.setSetpoint(clampedPosition, ControlType.kPosition);
+        motorController.setSetpoint(outputs.closedLoopTarget.getRadians(), ControlType.kPosition);
       }
       case OPEN_LOOP -> {
-        motor.set(MathUtil.clamp(outputs.openLoopOutput, -1.0, 1.0));
+        motor.set((MathUtil.clamp(outputs.openLoopOutput, -1.0, 1.0)));
       }
     }
   }
diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java
index b48db78..8cd8e06 100644
--- a/src/main/java/frc/robot/subsystems/vision/Vision.java
+++ b/src/main/java/frc/robot/subsystems/vision/Vision.java
@@ -16,6 +16,7 @@
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.subsystems.vision.CameraIO.PoseObservationType;
 import java.util.LinkedList;
@@ -63,6 +64,8 @@ public void periodic() {
       Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]);
     }
 
+    if (Timer.getTimestamp() % 2 < 0.07) System.out.println(inputs[0].connected);
+
     // Initialize logging values
     List allTagPoses = new LinkedList<>();
     List allRobotPoses = new LinkedList<>();
@@ -132,6 +135,7 @@ public void periodic() {
           angularStdDev *= VisionConstants.kCameraStdDevFactors[cameraIndex];
         }
 
+        // if (Timer.getTimestamp() % 2 < 0.07) System.out.println("Linear STDev " + linearStdDev);
         // Send vision observation
         consumer.accept(
             observation.pose().toPose2d(),

From fbf35307ba2acc8f2fab70d88b6642d7f9ba1565 Mon Sep 17 00:00:00 2001
From: Maxwell Morgan 
Date: Sat, 14 Mar 2026 13:31:56 -0400
Subject: [PATCH 50/61] Update hood

---
 .vscode/settings.json                         |   2 +-
 src/main/java/frc/robot/Constants.java        |   3 +-
 src/main/java/frc/robot/RobotContainer.java   |  79 +++++---
 src/main/java/frc/robot/RobotState.java       |   7 +
 .../subsystems/shooter/ShooterConstants.java  |   9 +-
 .../shooter/TrajectoryCalculator.java         |  21 +-
 .../robot/subsystems/shooter/hood/Hood.java   |  26 ++-
 .../shooter/hood/HoodIOSparkMax.java          |   3 +-
 .../subsystems/shooter/turret/Turret.java     |  57 +++---
 .../shooter/turret/TurretIOSparkMax.java      |  16 +-
 .../subsystems/vision/CameraIOLimelight.java  |   2 +-
 .../robot/util/LoggedDashboardChooser.java    | 184 ++++++++++++++++++
 12 files changed, 326 insertions(+), 83 deletions(-)
 create mode 100644 src/main/java/frc/robot/util/LoggedDashboardChooser.java

diff --git a/.vscode/settings.json b/.vscode/settings.json
index d139599..63de00c 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -70,5 +70,5 @@
   "[java]": {
     "editor.defaultFormatter": "redhat.java"
   },
-  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx32G -Xms100m -Xlog:disable"
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx64G -Xms100m -Xlog:disable"
 }
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 37bf02d..8ae139a 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -78,14 +78,13 @@ public static final class DeviceIDs {
         DriveConstants.TunerConstants.BackRight.EncoderId; // 22
 
     public static final int kLeftTurretFlywheel = 12;
-    public static final int kLeftTurretHood = 13;
+    public static final int kLeftTurretHood = 15;
     public static final int kLeftTurretAzimuth = 14;
 
     public static final int kRightTurretFlywheel = 9;
     public static final int kRightTurretHood = 10;
     public static final int kRightTurretAzimuth = 11;
 
-    public static final int kLeftGuts = 15;
     public static final int kRightGuts = 16;
 
     public static final int kIntakeDrive = 17;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 1b0735a..d9a5b08 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,24 +4,32 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
-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.RunCommand;
 import frc.robot.RobotState.OdometryObservation;
+import frc.robot.RobotState.VisionMeasurement;
 import frc.robot.control.DriverController;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.GyroIOPigeon2;
 import frc.robot.subsystems.drive.ModuleIO;
 import frc.robot.subsystems.shooter.Shooter.ShooterSide;
+import frc.robot.subsystems.shooter.hood.Hood;
+import frc.robot.subsystems.shooter.hood.HoodIOSparkMax;
 import frc.robot.subsystems.shooter.turret.Turret;
 import frc.robot.subsystems.shooter.turret.TurretIOSparkMax;
+import frc.robot.subsystems.vision.CameraIOLimelight;
 import frc.robot.subsystems.vision.Vision;
+import frc.robot.subsystems.vision.Vision.VisionConsumer;
+import frc.robot.util.AllianceFlipUtil;
+import frc.robot.util.FieldConstants;
 import java.util.function.Supplier;
 
 public class RobotContainer {
@@ -29,31 +37,37 @@ public class RobotContainer {
   private final DriverController operator = new DriverController.XboxDriverController(1);
 
   private Turret turret;
+  private Hood hood;
   private Vision vision;
   private Drive drive;
 
-  private Field2d field2d = new Field2d();
+  public static Field2d field2d = new Field2d();
 
   public RobotContainer() {
     turret = new Turret(ShooterSide.LEFT, new TurretIOSparkMax(ShooterSide.LEFT));
+    hood = new Hood(ShooterSide.LEFT, new HoodIOSparkMax(ShooterSide.LEFT));
+
     Supplier robotRotationSupplier = () -> RobotState.getInstance().getRotation();
-    // vision =
-    //     new Vision(
-    //         new VisionConsumer() {
-    //           public void accept(
-    //               Pose2d visionRobotPoseMeters,
-    //               double timestampSeconds,
-    //               edu.wpi.first.math.Matrix visionMeasurementStdDevs) {
-
-    //             RobotState.getInstance()
-    //                 .addVisionMeasurement(
-    //                     new VisionMeasurement(
-    //                         timestampSeconds, visionRobotPoseMeters, visionMeasurementStdDevs));
-    //           }
-    //           ;
-    //         },
-    //         new CameraIOLimelight("limelight-front", robotRotationSupplier),
-    //         new CameraIOLimelight("limelight", robotRotationSupplier));
+
+    SmartDashboard.putData("FieldInstance", field2d);
+    field2d.setRobotPose(RobotState.getInstance().getEstimatedPose());
+    vision =
+        new Vision(
+            new VisionConsumer() {
+              public void accept(
+                  Pose2d visionRobotPoseMeters,
+                  double timestampSeconds,
+                  edu.wpi.first.math.Matrix visionMeasurementStdDevs) {
+
+                RobotState.getInstance()
+                    .addVisionMeasurement(
+                        new VisionMeasurement(
+                            timestampSeconds, visionRobotPoseMeters, visionMeasurementStdDevs));
+              }
+              ;
+            },
+            new CameraIOLimelight("limelight-front", robotRotationSupplier),
+            new CameraIOLimelight("limelight-one", robotRotationSupplier));
     drive =
         new Drive(
             new GyroIOPigeon2(),
@@ -63,7 +77,18 @@ public RobotContainer() {
             new ModuleIO() {});
 
     turret.setDefaultCommand(
-        new RunCommand(() -> turret.setPosition(RobotState.getInstance().getRotation()), turret));
+        turret.trackTarget(
+            () -> AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d())));
+
+    // driver.aCross().onTrue(new InstantCommand(() -> hood.setAngle(0), hood));
+    // driver.bCircle().onTrue(new InstantCommand(() -> hood.setAngle(10), hood));
+
+    driver
+        .xSquare()
+        .whileTrue(
+            hood.trackTarget(
+                () ->
+                    AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d())));
 
     driver
         .rightBumper()
@@ -82,14 +107,10 @@ public void robotPeriodic() {
             new OdometryObservation(
                 Timer.getTimestamp(),
                 new SwerveModulePosition[] {
-                  new SwerveModulePosition(
-                      Math.random(), new Rotation2d(Math.random(), Math.random())),
-                  new SwerveModulePosition(
-                      Math.random(), new Rotation2d(Math.random(), Math.random())),
-                  new SwerveModulePosition(
-                      Math.random(), new Rotation2d(Math.random(), Math.random())),
-                  new SwerveModulePosition(
-                      Math.random(), new Rotation2d(Math.random(), Math.random()))
+                  new SwerveModulePosition(),
+                  new SwerveModulePosition(),
+                  new SwerveModulePosition(),
+                  new SwerveModulePosition()
                 },
                 drive.getRawGyroRotation()));
   }
diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java
index 90a8bd4..e57d4f1 100644
--- a/src/main/java/frc/robot/RobotState.java
+++ b/src/main/java/frc/robot/RobotState.java
@@ -4,6 +4,7 @@
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.numbers.N1;
@@ -26,6 +27,8 @@ public static RobotState getInstance() {
 
   private Rotation2d gyroOffset = new Rotation2d();
 
+  private Translation2d turretTarget = new Translation2d();
+
   private RobotState() {
     poseEstimator =
         new SwerveDrivePoseEstimator(
@@ -134,6 +137,10 @@ public ChassisSpeeds getFieldVelocity() {
     return ChassisSpeeds.fromRobotRelativeSpeeds(robotVelocity, getRotation());
   }
 
+  public Translation2d getTurretTarget() {
+      return turretTarget;
+  }
+
   public record OdometryObservation(
       double timestamp, SwerveModulePosition[] modulePositions, Rotation2d gyroAngle) {}
 
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index fc8f7e7..e4cba75 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -16,7 +16,7 @@ public final class ShooterConstants {
   public static final double kLatencySeconds = 0.05;
 
   public static final class TurretConstants {
-    public static final double kGearRatio = 10 / 1; // Motor / Turret
+    public static final double kGearRatio = 200 / 19; // Motor / Turret
     public static final double kMinTurretAngleRad = Units.degreesToRadians(-90);
     public static final double kMaxTurretAngleRad = Units.degreesToRadians(90);
     public static final double kAngleTolerance = Units.degreesToRadians(0.5);
@@ -34,7 +34,7 @@ public static final class TurretConstants {
 
   public static final class HoodConstants {
     public static final double kTurretToHoodInches = 1.878;
-    public static final double kGearRatio = 100 / 1;
+    public static final double kGearRatio = 19.2;
 
     public static final double kLeftHoodID = -1;
     public static final double kRightHoodID = -1;
@@ -69,16 +69,13 @@ public static final class HoodConstants {
                             Inches.of(-7.270121), Inches.of(0), Inches.of(0), new Rotation3d())));
 
     public static final double kMinAngleRad = Units.degreesToRadians(0);
-    public static final double kMaxAngleRad = Units.degreesToRadians(30);
+    public static final double kMaxAngleRad = 5.9;
   }
 
   public static final class FlywheelConstants {
     public static final double kGearRatio = 300;
     public static final double kSpeedTolerance = 25.0;
 
-    public static final int kLeftFlywheelID = -1;
-    public static final int kRightFlywheelID = -1;
-
     public static final Slot0Configs kGains =
         new Slot0Configs()
             .withKP(0.75)
diff --git a/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java
index 2d91069..75f180f 100644
--- a/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java
+++ b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java
@@ -24,14 +24,15 @@ public class TrajectoryCalculator {
   private static final double MAX_SHOOTING_DISTANCE = 5.0;
 
   static {
-    shooterTable.put(1.5, new TrajectoryParams(2800.0, 35.0, 0.38));
-    shooterTable.put(2.0, new TrajectoryParams(3100.0, 38.0, 0.45));
-    shooterTable.put(2.6289, new TrajectoryParams(5000.0, 42.0, 0.52));
-    shooterTable.put(3.0, new TrajectoryParams(3650.0, 46.0, 0.60));
-    shooterTable.put(3.5, new TrajectoryParams(3900.0, 50.0, 0.68));
-    shooterTable.put(4.0, new TrajectoryParams(4100.0, 54.0, 0.76));
-    shooterTable.put(4.5, new TrajectoryParams(4350.0, 58.0, 0.85));
-    shooterTable.put(5.0, new TrajectoryParams(4550.0, 62.0, 0.94));
+    shooterTable.put(1.5, new TrajectoryParams(2800.0, 0, 0.38));
+    shooterTable.put(2.0, new TrajectoryParams(3100.0, 0.0349, 0.45));
+    shooterTable.put(2.5, new TrajectoryParams(3250.0, 0.0698, 0.52));
+    shooterTable.put(3.0, new TrajectoryParams(3650.0, 0.104, 0.60));
+    shooterTable.put(3.5, new TrajectoryParams(3900.0, 0.1396, 0.68));
+    shooterTable.put(4.0, new TrajectoryParams(4100.0, 0.174, 0.76));
+    shooterTable.put(4.5, new TrajectoryParams(4350.0, 0.209, 0.85));
+    shooterTable.put(5.0, new TrajectoryParams(4550.0, 0.244, 0.94));
+    shooterTable.put(5.5, new TrajectoryParams(4550.0, 0.279, 1.05));
   }
 
   // ========== PUBLIC API ==========
@@ -49,6 +50,10 @@ public static double calculateRPM(Translation2d targetLocation, Pose2d robotPose
     return shooterTable.get(targetLocation.getDistance(robotPose.getTranslation())).wheelRPM;
   }
 
+  public static double calculateHoodAngle(Translation2d targetLocation, Pose2d robotPose) {
+    return 20 * shooterTable.get(targetLocation.getDistance(robotPose.getTranslation())).hoodAngle;
+  }
+
   /**
    * Calculate shooter commands for both shooters efficiently. Use this when both shooters need
    * calculation - avoids duplicate state queries.
diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java b/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java
index bb58518..5d99c7a 100644
--- a/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java
+++ b/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java
@@ -6,10 +6,17 @@
 
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.RobotState;
 import frc.robot.RobotVisualizer;
 import frc.robot.subsystems.shooter.Shooter.ShooterSide;
 import frc.robot.subsystems.shooter.ShooterConstants.HoodConstants;
+import frc.robot.subsystems.shooter.TrajectoryCalculator;
+import java.util.function.Supplier;
 import org.littletonrobotics.junction.Logger;
 
 public class Hood extends SubsystemBase {
@@ -18,6 +25,8 @@ public class Hood extends SubsystemBase {
   private final HoodIO io;
   private final HoodIOInputsAutoLogged inputs = new HoodIOInputsAutoLogged();
 
+  private double targetAngleRad = 0.0;
+
   private boolean atGoal = false;
   private Debouncer atGoalDebouncer = new Debouncer(0.2, DebounceType.kFalling);
 
@@ -37,6 +46,21 @@ public void periodic() {
     } else if (side == ShooterSide.RIGHT) {
       RobotVisualizer.getInstance().setRightHoodAngle(inputs.positionRad);
     }
+
+    io.setAngle(targetAngleRad);
+  }
+
+  public Command trackTarget(Supplier targetSupplier) {
+
+    return Commands.run(
+        () -> {
+          Translation2d target = targetSupplier.get();
+          Pose2d robotPose = RobotState.getInstance().getEstimatedPose();
+          setAngle(TrajectoryCalculator.calculateHoodAngle(target, robotPose));
+          Logger.recordOutput("Hood target angle", targetAngleRad);
+          Logger.recordOutput("Hood target difference", targetAngleRad - inputs.positionRad);
+        },
+        this);
   }
 
   /**
@@ -48,7 +72,7 @@ public void setAngle(double angle) {
     atGoal =
         atGoalDebouncer.calculate(
             Math.abs(angle - inputs.positionRad) < HoodConstants.kAngleTolerance);
-    io.setAngle(angle);
+    targetAngleRad = angle;
   }
 
   public void setOpenLoop(double output) {
diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
index 4540cb2..d75d26e 100644
--- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
@@ -46,7 +46,8 @@ public HoodIOSparkMax(ShooterSide side) {
         .positionConversionFactor(2 * Math.PI / HoodConstants.kGearRatio) // No absolute encoder...
         .velocityConversionFactor(2 * Math.PI / HoodConstants.kGearRatio / 60.0);
 
-    config.closedLoop.feedForward.kS(0);
+    config.closedLoop.feedForward.kS(0.015 * 12);
+    config.closedLoop.p(0.1);
 
     tryUntilOk(
         motor,
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
index b6cf435..65b1a31 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
@@ -12,6 +12,7 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.RobotContainer;
 import frc.robot.RobotState;
 import frc.robot.RobotVisualizer;
 import frc.robot.subsystems.shooter.Shooter.ShooterSide;
@@ -73,32 +74,36 @@ public void periodicAfterScheduler() {
   public Command trackTarget(Supplier targetSupplier) {
 
     return Commands.run(
-            () -> {
-              Translation2d target = targetSupplier.get();
-              Pose2d robotPose = RobotState.getInstance().getEstimatedPose();
-
-              Translation2d turretOffset = Translation2d.kZero;
-
-              // Turret position in field coordinates
-              Translation2d turretFieldPos =
-                  robotPose.getTranslation().plus(turretOffset.rotateBy(robotPose.getRotation()));
-
-              // Vector from turret -> target (field frame)
-              Translation2d deltaField = target.minus(turretFieldPos);
-              Logger.recordOutput("Turret Target Distance", deltaField.getNorm());
-
-              // Convert to robot frame
-              Translation2d deltaRobot = deltaField.rotateBy(robotPose.getRotation().unaryMinus());
-
-              // Angle turret should point (robot-relative)
-              Rotation2d targetAngle =
-                  Rotation2d.fromRadians(Math.atan2(deltaRobot.getY(), deltaRobot.getX()));
-
-              this.targetAngle = targetAngle;
-              setPosition(targetAngle);
-            },
-            this)
-        .until(() -> this.atGoal);
+        () -> {
+          Translation2d target = targetSupplier.get();
+          Pose2d robotPose = RobotState.getInstance().getEstimatedPose();
+          RobotContainer.field2d.setRobotPose(robotPose);
+
+          Translation2d turretOffset = Translation2d.kZero;
+
+          // Turret position in field coordinates
+          Translation2d turretFieldPos =
+              robotPose.getTranslation().plus(turretOffset.rotateBy(robotPose.getRotation()));
+
+          // Vector from turret -> target (field frame)
+          Translation2d deltaField = target.minus(turretFieldPos);
+          Logger.recordOutput("Turret Target Distance X", deltaField.getX());
+          Logger.recordOutput("Turret Target Distance", deltaField.getDistance(target));
+          Logger.recordOutput("Turret Target Norm", deltaField.getNorm());
+          Logger.recordOutput("Turret Target Distance Y", deltaField.getY());
+          Logger.recordOutput("Turret Target Angle?", deltaField.getAngle().getDegrees());
+
+          // Convert to robot frame
+          Translation2d deltaRobot = deltaField.rotateBy(robotPose.getRotation().unaryMinus());
+
+          // Angle turret should point (robot-relative)
+          Rotation2d targetAngle =
+              Rotation2d.fromRadians(Math.atan2(deltaRobot.getY(), deltaRobot.getX()));
+
+          this.targetAngle = targetAngle;
+          setPosition(targetAngle.unaryMinus());
+        },
+        this);
   }
 
   public Command zero() {
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
index 330b3af..af4875d 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
@@ -58,8 +58,8 @@ public TurretIOSparkMax(ShooterSide side) {
     config.softLimit.reverseSoftLimitEnabled(false).forwardSoftLimitEnabled(false);
 
     config.closedLoop.feedForward.kS(0.025 * 12);
-    config.closedLoop.p(0.7);
-    config.closedLoop.d(0.5);
+    config.closedLoop.p(0.1);
+    config.closedLoop.d(0.01);
     config.closedLoop.allowedClosedLoopError(
         TurretConstants.kAngleTolerance, ClosedLoopSlot.kSlot0);
 
@@ -89,13 +89,13 @@ public void updateInputs(TurretIOInputs inputs) {
   public void applyOutputs(TurretIOOutputs outputs) {
     switch (outputs.mode) {
       case CLOSED_LOOP -> {
-        // double clampedPosition =
-        //     MathUtil.clamp(
-        //         outputs.closedLoopTarget.getRadians(),
-        //         TurretConstants.kMinTurretAngleRad,
-        //         TurretConstants.kMaxTurretAngleRad);
+        double clampedPosition =
+            MathUtil.clamp(
+                outputs.closedLoopTarget.getRadians(),
+                TurretConstants.kMinTurretAngleRad,
+                TurretConstants.kMaxTurretAngleRad);
 
-        motorController.setSetpoint(outputs.closedLoopTarget.getRadians(), ControlType.kPosition);
+        motorController.setSetpoint(clampedPosition, ControlType.kPosition);
       }
       case OPEN_LOOP -> {
         motor.set((MathUtil.clamp(outputs.openLoopOutput, -1.0, 1.0)));
diff --git a/src/main/java/frc/robot/subsystems/vision/CameraIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/CameraIOLimelight.java
index f43f708..fffa366 100644
--- a/src/main/java/frc/robot/subsystems/vision/CameraIOLimelight.java
+++ b/src/main/java/frc/robot/subsystems/vision/CameraIOLimelight.java
@@ -140,7 +140,7 @@ public void updateInputs(CameraIOInputs inputs) {
   }
 
   /** Parses the 3D pose from a Limelight botpose array. */
-  private static Pose3d parsePose(double[] rawLLArray) {
+  public static Pose3d parsePose(double[] rawLLArray) {
     return new Pose3d(
         rawLLArray[0],
         rawLLArray[1],
diff --git a/src/main/java/frc/robot/util/LoggedDashboardChooser.java b/src/main/java/frc/robot/util/LoggedDashboardChooser.java
new file mode 100644
index 0000000..93e229d
--- /dev/null
+++ b/src/main/java/frc/robot/util/LoggedDashboardChooser.java
@@ -0,0 +1,184 @@
+// 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.
+
+// https://github.com/3015RangerRobotics/2024Public/blob/main/RobotCode2024/src/main/java/frc/robot/util/LoggedDashboardChooser.java
+
+package frc.robot.util;
+
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.lang.reflect.Field;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.function.Consumer;
+import org.littletonrobotics.junction.LogTable;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.inputs.LoggableInputs;
+import org.littletonrobotics.junction.networktables.LoggedNetworkInput;
+
+/**
+ * A dashboard chooser that integrates with AdvantageKit's logging system.
+ *
+ * 
This class wraps WPILib's SendableChooser to provide automatic logging of selected values.
+ * Unlike the standard SendableChooser, this version works correctly with AdvantageKit's replay
+ * functionality, allowing you to replay autonomous selections and other dashboard choices from log
+ * files.
+ *
+ * 
Example usage:
+ *
+ * 
{@code
+ * LoggedDashboardChooser autoChooser = new LoggedDashboardChooser<>("Auto Mode");
+ * autoChooser.addDefaultOption("Do Nothing", Commands.none());
+ * autoChooser.addOption("Simple Auto", simpleAuto());
+ * autoChooser.addOption("Complex Auto", complexAuto());
+ *
+ * // Get selected command
+ * Command selectedAuto = autoChooser.get();
+ * }

+ */
+public class LoggedDashboardChooser extends LoggedNetworkInput {
+  private final String key;
+  private String selectedValue = null;
+  private String lastSelected = null;
+  private SendableChooser sendableChooser = new SendableChooser<>();
+  private Map options = new HashMap<>();
+  private Consumer listener = null;
+
+  private final LoggableInputs inputs =
+      new LoggableInputs() {
+        @Override
+        public void toLog(LogTable table) {
+          table.put(key, selectedValue);
+        }
+
+        @Override
+        public void fromLog(LogTable table) {
+          selectedValue = table.get(key, selectedValue);
+        }
+      };
+
+  /**
+   * Creates a new LoggedDashboardChooser.
+   *
+   * @param key The SmartDashboard key, published to "/SmartDashboard/{key}" for NT or
+   *     "/DashboardInputs/{key}" when logged
+   */
+  public LoggedDashboardChooser(String key) {
+    this.key = key;
+    SmartDashboard.putData(key, sendableChooser);
+    periodic();
+    Logger.registerDashboardInput(this);
+  }
+
+  /**
+   * Creates a new LoggedDashboardChooser by copying options from an existing SendableChooser. Note:
+   * Updates to the original chooser after construction will not affect this object.
+   *
+   * @param key The SmartDashboard key for this chooser
+   * @param chooser Existing SendableChooser to copy options from
+   */
+  @SuppressWarnings("unchecked")
+  public LoggedDashboardChooser(String key, SendableChooser chooser) {
+    this(key);
+
+    // Get options map
+    Map options = new HashMap<>();
+    try {
+      Field mapField = SendableChooser.class.getDeclaredField("m_map");
+      mapField.setAccessible(true);
+      options = (Map) mapField.get(chooser);
+    } catch (NoSuchFieldException
+        | SecurityException
+        | IllegalArgumentException
+        | IllegalAccessException e) {
+      throw new IllegalStateException(e.getMessage());
+    }
+
+    // Get default option
+    String defaultString = "";
+    try {
+      Field defaultField = SendableChooser.class.getDeclaredField("m_defaultChoice");
+      defaultField.setAccessible(true);
+      defaultString = (String) defaultField.get(chooser);
+    } catch (NoSuchFieldException
+        | SecurityException
+        | IllegalArgumentException
+        | IllegalAccessException e) {
+      throw new IllegalStateException(e.getMessage());
+    }
+
+    // Add options
+    for (String optionKey : options.keySet()) {
+      if (optionKey.equals(defaultString)) {
+        addDefaultOption(optionKey, options.get(optionKey));
+      } else {
+        addOption(optionKey, options.get(optionKey));
+      }
+    }
+  }
+
+  /**
+   * Adds a new option to the chooser.
+   *
+   * @param key Display name for the option
+   * @param value Value returned when this option is selected
+   */
+  public void addOption(String key, V value) {
+    sendableChooser.addOption(key, key);
+    options.put(key, value);
+  }
+
+  /**
+   * Adds a new option and sets it as the default.
+   *
+   * @param key Display name for the default option
+   * @param value Value returned when this option is selected
+   */
+  public void addDefaultOption(String key, V value) {
+    sendableChooser.setDefaultOption(key, key);
+    options.put(key, value);
+  }
+
+  /**
+   * Returns the currently selected option value. If no option is selected, returns the default. If
+   * no default exists, returns null.
+   *
+   * @return The selected value, or null if nothing is selected
+   */
+  public V get() {
+    return options.get(selectedValue);
+  }
+
+  /**
+   * Returns the internal SendableChooser for dashboard layout configuration. Do not read data from
+   * this directly - use {@link #get()} instead.
+   *
+   * @return The internal SendableChooser object
+   */
+  public SendableChooser getSendableChooser() {
+    return sendableChooser;
+  }
+
+  @Override
+  public void periodic() {
+    if (!Logger.hasReplaySource()) {
+      selectedValue = sendableChooser.getSelected();
+    }
+    Logger.processInputs(prefix, inputs);
+
+    if (listener != null && !selectedValue.equals(lastSelected)) {
+      listener.accept(get());
+    }
+    lastSelected = selectedValue;
+  }
+
+  /**
+   * Registers a listener to be called when the selected option changes.
+   *
+   * @param listener Consumer to be called with the new value when selection changes
+   */
+  public void onChange(Consumer listener) {
+    this.listener = listener;
+  }
+}

From 5ba8bf9c815b31c535d2962a47d7dfbd6634aab9 Mon Sep 17 00:00:00 2001
From: Maxwell Morgan 
Date: Mon, 16 Mar 2026 15:55:43 -0400
Subject: [PATCH 51/61] Update turret/hood auto-aim

---
 src/main/java/frc/robot/Constants.java        |  12 +--
 src/main/java/frc/robot/RobotContainer.java   |  47 +++-----
 src/main/java/frc/robot/RobotState.java       |  17 ++-
 src/main/java/frc/robot/RobotVisualizer.java  |  84 ++++-----------
 .../frc/robot/control/DriverControls.java     | 101 ++++--------------
 .../java/frc/robot/control/ZoneControls.java  |  16 ++-
 .../frc/robot/subsystems/shooter/Shooter.java |  80 ++------------
 .../subsystems/shooter/ShooterConstants.java  |  32 ++----
 .../shooter/TrajectoryCalculator.java         |  41 ++-----
 .../subsystems/shooter/flywheel/Flywheel.java |  13 +--
 .../shooter/flywheel/FlywheelIOTalonFX.java   |  13 +--
 .../robot/subsystems/shooter/hood/Hood.java   |  18 +---
 .../shooter/hood/HoodIOSparkMax.java          |   7 +-
 .../subsystems/shooter/turret/Turret.java     |  26 ++---
 .../shooter/turret/TurretIOSparkMax.java      |  22 +---
 15 files changed, 132 insertions(+), 397 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 8ae139a..1646530 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -77,15 +77,11 @@ public static final class DeviceIDs {
     public static final int kBackRightModuleEncoder =
         DriveConstants.TunerConstants.BackRight.EncoderId; // 22
 
-    public static final int kLeftTurretFlywheel = 12;
-    public static final int kLeftTurretHood = 15;
-    public static final int kLeftTurretAzimuth = 14;
+    public static final int kTurretFlywheel = 12;
+    public static final int kTurretHood = 15;
+    public static final int kTurretAzimuth = 14;
 
-    public static final int kRightTurretFlywheel = 9;
-    public static final int kRightTurretHood = 10;
-    public static final int kRightTurretAzimuth = 11;
-
-    public static final int kRightGuts = 16;
+    public static final int kGuts = 16;
 
     public static final int kIntakeDrive = 17;
     public static final int kIntakePivot = 18;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index d9a5b08..18af04d 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -20,7 +20,8 @@
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.GyroIOPigeon2;
 import frc.robot.subsystems.drive.ModuleIO;
-import frc.robot.subsystems.shooter.Shooter.ShooterSide;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.hood.Hood;
 import frc.robot.subsystems.shooter.hood.HoodIOSparkMax;
 import frc.robot.subsystems.shooter.turret.Turret;
@@ -28,29 +29,35 @@
 import frc.robot.subsystems.vision.CameraIOLimelight;
 import frc.robot.subsystems.vision.Vision;
 import frc.robot.subsystems.vision.Vision.VisionConsumer;
-import frc.robot.util.AllianceFlipUtil;
-import frc.robot.util.FieldConstants;
+import frc.robot.util.GeomUtil;
 import java.util.function.Supplier;
 
 public class RobotContainer {
   private final DriverController driver = new DriverController.XboxDriverController(0);
   private final DriverController operator = new DriverController.XboxDriverController(1);
 
-  private Turret turret;
-  private Hood hood;
   private Vision vision;
   private Drive drive;
 
   public static Field2d field2d = new Field2d();
+  public static Field2d targetField2d = new Field2d();
 
   public RobotContainer() {
-    turret = new Turret(ShooterSide.LEFT, new TurretIOSparkMax(ShooterSide.LEFT));
-    hood = new Hood(ShooterSide.LEFT, new HoodIOSparkMax(ShooterSide.LEFT));
 
     Supplier robotRotationSupplier = () -> RobotState.getInstance().getRotation();
 
     SmartDashboard.putData("FieldInstance", field2d);
+    SmartDashboard.putData("TargetField", targetField2d);
     field2d.setRobotPose(RobotState.getInstance().getEstimatedPose());
+    switch (Constants.kCurrentMode) {
+      case REAL:
+        
+      case SIM:
+
+      case REPLAY:
+      default:
+        
+    }
     vision =
         new Vision(
             new VisionConsumer() {
@@ -75,30 +82,6 @@ public void accept(
             new ModuleIO() {},
             new ModuleIO() {},
             new ModuleIO() {});
-
-    turret.setDefaultCommand(
-        turret.trackTarget(
-            () -> AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d())));
-
-    // driver.aCross().onTrue(new InstantCommand(() -> hood.setAngle(0), hood));
-    // driver.bCircle().onTrue(new InstantCommand(() -> hood.setAngle(10), hood));
-
-    driver
-        .xSquare()
-        .whileTrue(
-            hood.trackTarget(
-                () ->
-                    AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d())));
-
-    driver
-        .rightBumper()
-        .whileTrue(
-            Commands.runEnd(() -> turret.setOpenLoop(0.1), () -> turret.setOpenLoop(0), turret));
-
-    driver
-        .leftBumper()
-        .whileTrue(
-            Commands.runEnd(() -> turret.setOpenLoop(-0.1), () -> turret.setOpenLoop(0), turret));
   }
 
   public void robotPeriodic() {
@@ -113,6 +96,8 @@ public void robotPeriodic() {
                   new SwerveModulePosition()
                 },
                 drive.getRawGyroRotation()));
+
+    targetField2d.setRobotPose(GeomUtil.toPose2d(RobotState.getInstance().getTurretTarget()));
   }
 
   public Command getAutonomousCommand() {
diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java
index e57d4f1..2e29a77 100644
--- a/src/main/java/frc/robot/RobotState.java
+++ b/src/main/java/frc/robot/RobotState.java
@@ -1,5 +1,7 @@
 package frc.robot;
 
+import static edu.wpi.first.units.Units.Meters;
+
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -10,6 +12,8 @@
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import frc.robot.subsystems.drive.DriveConstants;
+import frc.robot.util.AllianceFlipUtil;
+import frc.robot.util.FieldConstants;
 import org.littletonrobotics.junction.Logger;
 
 public class RobotState {
@@ -27,8 +31,6 @@ public static RobotState getInstance() {
 
   private Rotation2d gyroOffset = new Rotation2d();
 
-  private Translation2d turretTarget = new Translation2d();
-
   private RobotState() {
     poseEstimator =
         new SwerveDrivePoseEstimator(
@@ -138,7 +140,16 @@ public ChassisSpeeds getFieldVelocity() {
   }
 
   public Translation2d getTurretTarget() {
-      return turretTarget;
+    Pose2d estimatedPose = getEstimatedPose();
+    if (estimatedPose.getX()
+        < AllianceFlipUtil.applyX(FieldConstants.LinesVertical.neutralZoneNear)) {
+      if (estimatedPose.getY() > AllianceFlipUtil.applyY(FieldConstants.LinesHorizontal.center)) {
+        return AllianceFlipUtil.apply(new Translation2d(Meters.of(2), Meters.of(1)));
+      }
+      return AllianceFlipUtil.apply(
+          new Translation2d(Meters.of(2), Meters.of(FieldConstants.fieldWidth - 1)));
+    }
+    return AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d());
   }
 
   public record OdometryObservation(
diff --git a/src/main/java/frc/robot/RobotVisualizer.java b/src/main/java/frc/robot/RobotVisualizer.java
index b7e21e9..9afe1aa 100644
--- a/src/main/java/frc/robot/RobotVisualizer.java
+++ b/src/main/java/frc/robot/RobotVisualizer.java
@@ -20,8 +20,8 @@ public static RobotVisualizer getInstance() {
     return instance;
   }
 
-  private Rotation2d[] turretAngles = {Rotation2d.kZero, Rotation2d.kZero}; // Left, Right
-  private double[] hoodAngles = {0.0, 0.0}; // Left, Right
+  private Rotation2d turretAngle = Rotation2d.kZero;
+  private double hoodAngle = 0.0;
 
   private RobotVisualizer() {}
 
@@ -31,33 +31,21 @@ private RobotVisualizer() {}
    * @param key A String representing the output location.
    */
   public void log(String key) {
-    Pose3d leftTurretPose =
-        GeomUtil.toPose3d(TurretConstants.kRobotToLeftTurret)
+    Pose3d turretPose =
+        GeomUtil.toPose3d(TurretConstants.kRobotToTurret)
             .transformBy(
                 new Transform3d(
                     Translation3d.kZero,
-                    new Rotation3d(0.0, 0.0, turretAngles[0].plus(Rotation2d.kPi).getRadians())));
-    Pose3d rightTurretPose =
-        GeomUtil.toPose3d(TurretConstants.kRobotToRightTurret)
-            .transformBy(
-                new Transform3d(
-                    Translation3d.kZero,
-                    new Rotation3d(0.0, 0.0, turretAngles[1].plus(Rotation2d.kPi).getRadians())));
+                    new Rotation3d(0.0, 0.0, turretAngle.plus(Rotation2d.kPi).getRadians())));
 
-    Pose3d leftHoodPose =
-        leftTurretPose.transformBy(
+    Pose3d hoodPose =
+        turretPose.transformBy(
             new Transform3d(
-                HoodConstants.kLeftTurretToLeftHood.getTranslation(),
-                new Rotation3d(0.0, hoodAngles[0], 0.0)));
-
-    Pose3d rightHoodPose =
-        rightTurretPose.transformBy(
-            new Transform3d(
-                HoodConstants.kRightTurretToRightHood.getTranslation(),
-                new Rotation3d(0.0, hoodAngles[1], 0.0)));
+                HoodConstants.kTurretToHood.getTranslation(),
+                new Rotation3d(0.0, hoodAngle, 0.0)));
 
     Logger.recordOutput(
-        key + "/Components", leftTurretPose, rightTurretPose, leftHoodPose, rightHoodPose);
+        key + "/Components", turretPose, hoodPose);
   }
 
   /**
@@ -65,17 +53,8 @@ public void log(String key) {
    *
    * @return A Rotation2d object representing the left turret angle.
    */
-  public Rotation2d getLeftTurretAngle() {
-    return turretAngles[0];
-  }
-
-  /**
-   * Gets the right turret angle.
-   *
-   * @return A Rotation2d object representing the right turret angle.
-   */
-  public Rotation2d getRightTurretAngle() {
-    return turretAngles[1];
+  public Rotation2d getTurretAzimuthAngle() {
+    return turretAngle;
   }
 
   /**
@@ -83,17 +62,8 @@ public Rotation2d getRightTurretAngle() {
    *
    * @param angle A Rotation2d object to be inserted in the angles array.
    */
-  public void setLeftTurretAngle(Rotation2d angle) {
-    turretAngles[0] = angle;
-  }
-
-  /**
-   * Sets the right turret angle.
-   *
-   * @param angle A Rotation2d object to be inserted in the angles array.
-   */
-  public void setRightTurretAngle(Rotation2d angle) {
-    turretAngles[1] = angle;
+  public void setTurretAzimuthAngle(Rotation2d angle) {
+    turretAngle = angle;
   }
 
   /**
@@ -101,17 +71,8 @@ public void setRightTurretAngle(Rotation2d angle) {
    *
    * @return A double representing the left hood angle in radians.
    */
-  public double getLeftHoodAngle() {
-    return hoodAngles[0];
-  }
-
-  /**
-   * Gets the left hood angle.
-   *
-   * @return A double representing the right hood angle in radians.
-   */
-  public double getRightHoodAngle() {
-    return hoodAngles[1];
+  public double getTurretHoodAngle() {
+    return hoodAngle;
   }
 
   /**
@@ -119,16 +80,7 @@ public double getRightHoodAngle() {
    *
    * @param angle A Rotation2d object to be inserted in the angles array.
    */
-  public void setLeftHoodAngle(double angle) {
-    hoodAngles[0] = angle;
-  }
-
-  /**
-   * Sets the right hood angle in radians.
-   *
-   * @param angle A double to be inserted in the angles array.
-   */
-  public void setRightHoodAngle(double angle) {
-    hoodAngles[1] = angle;
+  public void setTurretHoodAngle(double angle) {
+    hoodAngle = angle;
   }
 }
diff --git a/src/main/java/frc/robot/control/DriverControls.java b/src/main/java/frc/robot/control/DriverControls.java
index fce73a2..87ea624 100644
--- a/src/main/java/frc/robot/control/DriverControls.java
+++ b/src/main/java/frc/robot/control/DriverControls.java
@@ -1,30 +1,15 @@
 package frc.robot.control;
 
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.StartEndCommand;
-import frc.robot.RobotState;
 import frc.robot.commands.DriveCommands;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.guts.Guts;
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.shooter.Shooter;
-import frc.robot.util.AllianceFlipUtil;
 import frc.robot.util.Direction;
-import frc.robot.util.FieldConstants;
-import org.littletonrobotics.junction.AutoLogOutput;
 
 public class DriverControls implements Configurable {
-  @AutoLogOutput(key = "Control/DriverControls/mode")
-  private DriverMode mode = DriverMode.TWO_DRIVERS;
-
-  public enum DriverMode {
-    ONE_DRIVER,
-    TWO_DRIVERS;
-  }
-
   private final DriverController driver;
   private final DriverController operator;
   private final Drive drive;
@@ -55,8 +40,11 @@ public DriverControls(
 
   @Override
   public void configure() {
+    configureDriverControls();
+    configureOperatorControls();
+  }
 
-    // Neutral controls (regardless of whether we are in one or two driver mode)
+  private void configureDriverControls() {
     driver.xSquare().onTrue(Commands.runOnce(drive::zeroYaw, drive));
     driver.bCircle().onTrue(Commands.runOnce(drive::stopWithX, drive));
 
@@ -69,23 +57,25 @@ public void configure() {
     driver.dPadDownRight().whileTrue(DriveCommands.crabWalk(drive, Direction.SOUTHEAST));
     driver.dPadDown().whileTrue(DriveCommands.crabWalk(drive, Direction.SOUTH));
 
-    driver
-        .leftBumper()
-        .whileTrue(
-            DriveCommands.joystickDriveAtAngle(
-                drive,
-                () -> -driver.getLeftY(), // xSupplier
-                () -> -driver.getLeftX(), // ySupplier
-                () -> {
-                  Pose2d robotPose = RobotState.getInstance().getEstimatedPose();
-                  Translation2d target =
-                      AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d());
-
-                  Translation2d delta = target.minus(robotPose.getTranslation());
-
-                  return new Rotation2d(Math.atan2(delta.getY(), delta.getX()));
-                }));
+    // driver
+    // .leftBumper()
+    // .whileTrue(
+    // DriveCommands.joystickDriveAtAngle(
+    // drive,
+    // () -> -driver.getLeftY(), // xSupplier
+    // () -> -driver.getLeftX(), // ySupplier
+    // () -> {
+    // Pose2d robotPose = RobotState.getInstance().getEstimatedPose();
+    // Translation2d target =
+    // AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d());
+
+    // Translation2d delta = target.minus(robotPose.getTranslation());
+
+    // return new Rotation2d(Math.atan2(delta.getY(), delta.getX()));
+    // }));
+  }
 
+  private void configureOperatorControls() {
     operator.leftBumper().and(operator.leftTrigger().negate()).whileTrue(intake.intake());
 
     operator
@@ -139,51 +129,4 @@ public void configure() {
                     rightGuts.runGutForward(),
                     leftGuts.runGutForward()));
   }
-
-  /*
-   * Driver Bindings:
-   *
-   * LB: Toggle deploy/retract intake
-   * LT: Spin intake
-   * RB: Shoot
-   * LT + A: backspin intake
-   * RT: Climb RT + A: Unclimb
-   * X: reset Gyro
-   * D-Pad: CrabWalk
-   * LB + RB + Y: Aux Handoff
-   *
-   */
-  private void configureOneDriver() {}
-
-  /*
-   * 
Back up Operator Controls:
-   *
-   * 
Pancake up + down: Pitch of turrets Pancake left + right: rotation of
-   * turrets trigger
-   * button: Fires fuel from turrets
-   *
-   * 
button 7: deploy intake button 8: run intake button 9: retract intake
-   *
-   * 
button 6: climber up button 4: climber down
-   *
-   * 
thumb button: Driver Handoff
-   */
-  private void configureTwoDrivers() {}
-
-  private boolean isOneDriver() {
-    return mode == DriverMode.ONE_DRIVER;
-  }
-
-  private boolean isTwoDrivers() {
-    return mode == DriverMode.TWO_DRIVERS;
-  }
-
-  public void setMode(DriverMode mode) {
-    this.mode = mode;
-    configure();
-  }
-
-  public DriverMode getMode() {
-    return mode;
-  }
 }
diff --git a/src/main/java/frc/robot/control/ZoneControls.java b/src/main/java/frc/robot/control/ZoneControls.java
index d5d6081..3a16408 100644
--- a/src/main/java/frc/robot/control/ZoneControls.java
+++ b/src/main/java/frc/robot/control/ZoneControls.java
@@ -1,10 +1,22 @@
 package frc.robot.control;
 
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import frc.robot.util.FieldConstants;
+import frc.robot.util.Zone;
+
 public class ZoneControls implements Configurable {
 
+  private Zone leftTrenchZone = new Zone.RectangleZone(FieldConstants.LeftTrench.openingTopLeft.toTranslation2d(),
+      FieldConstants.LeftTrench.openingTopRight.toTranslation2d()
+          .plus(new Translation2d(FieldConstants.LeftTrench.depth, 0)));
+
   @Override
   public void configure() {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'configure'");
+    Logger.recordOutput("TrenchZoneLeft",
+        new Translation2d[] { FieldConstants.LeftTrench.openingTopLeft.toTranslation2d(),
+            FieldConstants.LeftTrench.openingTopRight.toTranslation2d()
+                .plus(new Translation2d(FieldConstants.LeftTrench.depth, 0)) });
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index a379f46..721838a 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -21,25 +21,15 @@
 import java.util.function.Supplier;
 
 public class Shooter extends SubsystemBase {
-  private final ShooterSide side;
-
   private Turret turret;
   private Hood hood;
   private Flywheel flywheel;
 
   /** Creates a new Shooter. */
-  public Shooter(ShooterSide side, TurretIO turretIO, HoodIO hoodIO, FlywheelIO flywheelIO) {
-    this.side = side;
-    this.turret = new Turret(side, turretIO);
-    this.hood = new Hood(side, hoodIO);
-    this.flywheel = new Flywheel(side, flywheelIO);
-  }
-
-  public Shooter(ShooterSide side, HoodIO hoodIO, FlywheelIO flywheelIO) {
-    this.side = side;
-    this.turret = null;
-    this.hood = new Hood(side, hoodIO);
-    this.flywheel = new Flywheel(side, flywheelIO);
+  public Shooter(TurretIO turretIO, HoodIO hoodIO, FlywheelIO flywheelIO) {
+    this.turret = new Turret(turretIO);
+    this.hood = new Hood(hoodIO);
+    this.flywheel = new Flywheel(flywheelIO);
   }
 
   @Override
@@ -51,45 +41,6 @@ public void periodic() {
     flywheel.periodic();
   }
 
-  public static Command shootBothAtHub(Shooter leftShooter, Shooter rightShooter) {
-    return shootBothAtTarget(
-        leftShooter,
-        rightShooter,
-        () -> AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d()));
-  }
-
-  /**
-   * Calculate and apply trajectory parameters for both shooters.
-   *
-   * @param leftShooter The left shooter subsystem.
-   * @param rightShooter The right shooter subsystem.
-   * @param targetSupplier A supplier for the target.
-   * @return A RunCommand applying trajectory parameters to both shooters.
-   */
-  public static Command shootBothAtTarget(
-      Shooter leftShooter, Shooter rightShooter, Supplier targetSupplier) {
-    return Commands.run(
-        () -> {
-          var cmds = TrajectoryCalculator.calculateBoth(targetSupplier.get());
-          leftShooter.applyCommand(cmds.left());
-          rightShooter.applyCommand(cmds.right());
-        },
-        leftShooter,
-        rightShooter);
-  }
-
-  public static Command shootBothAtTargetNoTurret(
-      Shooter leftShooter, Shooter rightShooter, Supplier targetSupplier) {
-    return Commands.run(
-        () -> {
-          var cmds = TrajectoryCalculator.calculateBoth(targetSupplier.get());
-          leftShooter.applyCommandNoRotation(cmds.left());
-          rightShooter.applyCommandNoRotation(cmds.right());
-        },
-        leftShooter,
-        rightShooter);
-  }
-
   /**
    * Apply a pre-calculated shooter command to this shooter. This does not require the shooter
    * subsystem - use when combining with other shooters.
@@ -112,7 +63,7 @@ public void applyCommandNoRotation(ShooterCommand cmd) {
   public Command shootAtTargetRotation(Supplier targetSupplier) {
     return Commands.run(
         () -> {
-          ShooterCommand cmd = TrajectoryCalculator.calculate(side, targetSupplier.get());
+          ShooterCommand cmd = TrajectoryCalculator.calculate(targetSupplier.get());
           flywheel.setVelocity(cmd.wheelRPM());
           hood.setAngle(cmd.hoodAngle());
           turret.setPosition(cmd.turretAngle());
@@ -126,7 +77,7 @@ public Command shootAtTargetRotation(Supplier targetSupplier) {
   public Command shootAtTargetNoRotation(Supplier targetSupplier) {
     return Commands.run(
         () -> {
-          ShooterCommand cmd = TrajectoryCalculator.calculate(side, targetSupplier.get());
+          ShooterCommand cmd = TrajectoryCalculator.calculate(targetSupplier.get());
           flywheel.setVelocity(cmd.wheelRPM());
           hood.setAngle(cmd.hoodAngle());
         },
@@ -166,23 +117,4 @@ public void setHoodOpenLoop(double output) {
   public void setTurretOpenLoop(double output) {
     turret.setOpenLoop(output);
   }
-
-  public ShooterSide getSide() {
-    return side;
-  }
-
-  public enum ShooterSide {
-    LEFT("Left"),
-    RIGHT("Right");
-
-    private String name;
-
-    private ShooterSide(String name) {
-      this.name = name;
-    }
-
-    public String getName() {
-      return name;
-    }
-  }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index e4cba75..9dc80b3 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -17,19 +17,16 @@ public final class ShooterConstants {
 
   public static final class TurretConstants {
     public static final double kGearRatio = 200 / 19; // Motor / Turret
-    public static final double kMinTurretAngleRad = Units.degreesToRadians(-90);
-    public static final double kMaxTurretAngleRad = Units.degreesToRadians(90);
+    public static final double kMinTurretAngleRad = Units.degreesToRadians(-120);
+    public static final double kMaxTurretAngleRad = Units.degreesToRadians(120);
     public static final double kAngleTolerance = Units.degreesToRadians(0.5);
 
     public static final double kLeftMotorId = 12;
     public static final double kRightMotorId = 13;
 
     // +X = Forward, +Y = Left
-    public static final Transform3d kRobotToLeftTurret =
+    public static final Transform3d kRobotToTurret =
         new Transform3d(Inches.of(3.749), Inches.of(8.186), Inches.of(13.401), Rotation3d.kZero);
-
-    public static final Transform3d kRobotToRightTurret =
-        new Transform3d(Inches.of(3.749), Inches.of(-8.314), Inches.of(13.401), Rotation3d.kZero);
   }
 
   public static final class HoodConstants {
@@ -41,33 +38,18 @@ public static final class HoodConstants {
 
     public static final double kAngleTolerance = Units.degreesToRadians(5);
 
-    public static final Transform3d kRobotToLeftHood =
+    public static final Transform3d kRobotToHood =
         new Transform3d(
             Inches.of(7.268715), Meters.of(0.20792316), Inches.of(16.018516), Rotation3d.kZero);
 
-    public static final Transform3d kRobotToRightHood =
-        new Transform3d(
-            Inches.of(-7.270121),
-            Inches.of(-(12.062888 - (7.5 / 2.0))),
-            Inches.of(16.018516),
-            Rotation3d.kZero);
-
-    public static final Transform3d kLeftTurretToLeftHood =
-        GeomUtil.toPose3d(HoodConstants.kRobotToLeftHood)
+    public static final Transform3d kTurretToHood =
+        GeomUtil.toPose3d(HoodConstants.kRobotToHood)
             .minus(
-                GeomUtil.toPose3d(TurretConstants.kRobotToLeftTurret)
+                GeomUtil.toPose3d(TurretConstants.kRobotToTurret)
                     .plus(
                         new Transform3d(
                             Inches.of(7.268715), Inches.of(0), Inches.of(0), new Rotation3d())));
 
-    public static final Transform3d kRightTurretToRightHood =
-        GeomUtil.toPose3d(HoodConstants.kRobotToRightHood)
-            .minus(
-                GeomUtil.toPose3d(TurretConstants.kRobotToRightTurret)
-                    .plus(
-                        new Transform3d(
-                            Inches.of(-7.270121), Inches.of(0), Inches.of(0), new Rotation3d())));
-
     public static final double kMinAngleRad = Units.degreesToRadians(0);
     public static final double kMaxAngleRad = 5.9;
   }
diff --git a/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java
index 75f180f..75086f3 100644
--- a/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java
+++ b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java
@@ -10,7 +10,6 @@
 import edu.wpi.first.math.interpolation.InverseInterpolator;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import frc.robot.RobotState;
-import frc.robot.subsystems.shooter.Shooter.ShooterSide;
 import frc.robot.subsystems.shooter.ShooterConstants.TurretConstants;
 import frc.robot.util.GeomUtil;
 import org.littletonrobotics.junction.Logger;
@@ -41,9 +40,9 @@ public class TrajectoryCalculator {
    * Calculate shooter command for a single shooter. Use this when only one shooter needs
    * calculation.
    */
-  public static ShooterCommand calculate(ShooterSide side, Translation2d targetLocation) {
+  public static ShooterCommand calculate(Translation2d targetLocation) {
     RobotStateData state = getCompensatedRobotState();
-    return calculateWithState(side, targetLocation, state);
+    return calculateWithState(targetLocation, state);
   }
 
   public static double calculateRPM(Translation2d targetLocation, Pose2d robotPose) {
@@ -54,17 +53,6 @@ public static double calculateHoodAngle(Translation2d targetLocation, Pose2d rob
     return 20 * shooterTable.get(targetLocation.getDistance(robotPose.getTranslation())).hoodAngle;
   }
 
-  /**
-   * Calculate shooter commands for both shooters efficiently. Use this when both shooters need
-   * calculation - avoids duplicate state queries.
-   */
-  public static DualShooterCommands calculateBoth(Translation2d targetLocation) {
-    RobotStateData state = getCompensatedRobotState();
-    return new DualShooterCommands(
-        calculateWithState(ShooterSide.LEFT, targetLocation, state),
-        calculateWithState(ShooterSide.RIGHT, targetLocation, state));
-  }
-
   // ========== PRIVATE IMPLEMENTATION ==========
 
   /** Get and compensate robot state (shared between both shooters). */
@@ -84,21 +72,10 @@ private static RobotStateData getCompensatedRobotState() {
   }
 
   /** Calculate shooter command for a specific side using pre-computed robot state. */
-  private static ShooterCommand calculateWithState(
-      ShooterSide side, Translation2d targetLocation, RobotStateData state) {
+  private static ShooterCommand calculateWithState(Translation2d targetLocation, RobotStateData state) {
 
     // 2. Identify Turret Offset and Position
-    Transform3d robotToTurret;
-    switch (side) {
-      case LEFT:
-        robotToTurret = TurretConstants.kRobotToLeftTurret;
-        break;
-      case RIGHT:
-        robotToTurret = TurretConstants.kRobotToRightTurret;
-      default:
-        robotToTurret = new Transform3d();
-        break;
-    }
+    Transform3d robotToTurret = TurretConstants.kRobotToTurret;
 
     Pose2d turretPose =
         state.compensatedRobotPose.transformBy(GeomUtil.toTransform2d(robotToTurret));
@@ -141,15 +118,15 @@ private static ShooterCommand calculateWithState(
         lookaheadTurretPose.transformBy(GeomUtil.toTransform2d(robotToTurret).inverse());
 
     Logger.recordOutput(
-        "LaunchCalculator/" + side.getName() + "/LookaheadRobotPose", lookaheadRobotPose);
+        "LaunchCalculator/LookaheadRobotPose", lookaheadRobotPose);
     Logger.recordOutput(
-        "LaunchCalculator/" + side.getName() + "/ShotVector",
+        "LaunchCalculator/ShotVector",
         new Pose2d(lookaheadRobotPose.getTranslation(), turretAngleField));
-    Logger.recordOutput("LaunchCalculator/" + side.getName() + "/Distance", lookaheadDistance);
+    Logger.recordOutput("LaunchCalculator/Distance", lookaheadDistance);
     Logger.recordOutput(
-        "LaunchCalculator/" + side.getName() + "/DistanceClamped", clampedFinalDistance);
+        "LaunchCalculator/DistanceClamped", clampedFinalDistance);
     Logger.recordOutput(
-        "LaunchCalculator/" + side.getName() + "/IsInRange",
+        "LaunchCalculator/IsInRange",
         lookaheadDistance >= MIN_SHOOTING_DISTANCE && lookaheadDistance <= MAX_SHOOTING_DISTANCE);
 
     return new ShooterCommand(params.wheelRPM(), params.hoodAngle(), turretAngleRobot);
diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java
index 6cdd1c4..4ebfe75 100644
--- a/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java
+++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java
@@ -11,12 +11,10 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.subsystems.shooter.Shooter.ShooterSide;
 import frc.robot.subsystems.shooter.ShooterConstants.FlywheelConstants;
 import org.littletonrobotics.junction.Logger;
 
 public class Flywheel extends SubsystemBase {
-  private final ShooterSide side;
 
   private final FlywheelIO io;
   private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged();
@@ -26,16 +24,15 @@ public class Flywheel extends SubsystemBase {
   private double goalRPM = 0.0;
 
   /** Creates a new Flywheel. */
-  public Flywheel(ShooterSide side, FlywheelIO io) {
-    this.side = side;
+  public Flywheel(FlywheelIO io) {
     this.io = io;
   }
 
   @Override
   public void periodic() {
     io.updateInputs(inputs);
-    Logger.processInputs("Shooter/" + side.getName() + "/Flywheel", inputs);
-    Logger.recordOutput("Shooter/" + side.getName() + "/Flywheel/AtGoal", atGoal);
+    Logger.processInputs("Shooter/Flywheel", inputs);
+    Logger.recordOutput("Shooter/Flywheel/AtGoal", atGoal);
 
     SmartDashboard.putNumber("Flywheel Velo", getVelocity());
     SmartDashboard.putNumber("Flywheel Setpoint", goalRPM);
@@ -80,8 +77,4 @@ public void stop() {
   public double getVelocity() {
     return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec);
   }
-
-  public ShooterSide getSide() {
-    return this.side;
-  }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java
index 23c659b..3d085db 100644
--- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java
@@ -15,7 +15,6 @@
 import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.units.measure.Voltage;
 import frc.robot.Constants.DeviceIDs;
-import frc.robot.subsystems.shooter.Shooter.ShooterSide;
 import frc.robot.subsystems.shooter.ShooterConstants.FlywheelConstants;
 
 public class FlywheelIOTalonFX implements FlywheelIO {
@@ -29,20 +28,12 @@ public class FlywheelIOTalonFX implements FlywheelIO {
 
   private final VelocityVoltage velocityRequest = new VelocityVoltage(0).withSlot(0);
 
-  public FlywheelIOTalonFX(ShooterSide side) {
+  public FlywheelIOTalonFX() {
     motor =
         new TalonFX(
-            side == ShooterSide.LEFT
-                ? DeviceIDs.kLeftTurretFlywheel
-                : DeviceIDs.kRightTurretFlywheel);
+            DeviceIDs.kTurretFlywheel);
     motorConfig =
         new TalonFXConfiguration()
-            .withMotorOutput(
-                new MotorOutputConfigs()
-                    .withInverted(
-                        side == ShooterSide.RIGHT
-                            ? InvertedValue.Clockwise_Positive
-                            : InvertedValue.CounterClockwise_Positive))
             .withSlot0(FlywheelConstants.kGains)
             /**
              * TODO: Update gains Peiwei, Ben: see the FlywheelConstants.kGains above... thats where
diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java b/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java
index 5d99c7a..0174ecf 100644
--- a/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java
+++ b/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java
@@ -13,15 +13,12 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.RobotState;
 import frc.robot.RobotVisualizer;
-import frc.robot.subsystems.shooter.Shooter.ShooterSide;
 import frc.robot.subsystems.shooter.ShooterConstants.HoodConstants;
 import frc.robot.subsystems.shooter.TrajectoryCalculator;
 import java.util.function.Supplier;
 import org.littletonrobotics.junction.Logger;
 
 public class Hood extends SubsystemBase {
-  private final ShooterSide side;
-
   private final HoodIO io;
   private final HoodIOInputsAutoLogged inputs = new HoodIOInputsAutoLogged();
 
@@ -31,21 +28,16 @@ public class Hood extends SubsystemBase {
   private Debouncer atGoalDebouncer = new Debouncer(0.2, DebounceType.kFalling);
 
   /** Creates a new Hood. */
-  public Hood(ShooterSide side, HoodIO io) {
-    this.side = side;
+  public Hood(HoodIO io) {
     this.io = io;
   }
 
   @Override
   public void periodic() {
     io.updateInputs(inputs);
-    Logger.processInputs(("Hood/" + side.getName()), inputs);
+    Logger.processInputs("Hood", inputs);
 
-    if (side == ShooterSide.LEFT) {
-      RobotVisualizer.getInstance().setLeftHoodAngle(inputs.positionRad);
-    } else if (side == ShooterSide.RIGHT) {
-      RobotVisualizer.getInstance().setRightHoodAngle(inputs.positionRad);
-    }
+    RobotVisualizer.getInstance().setTurretHoodAngle(inputs.positionRad);
 
     io.setAngle(targetAngleRad);
   }
@@ -90,8 +82,4 @@ public double getVelocity() {
   public boolean atGoal() {
     return atGoal;
   }
-
-  public ShooterSide getSide() {
-    return this.side;
-  }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
index d75d26e..c9a8420 100644
--- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
@@ -17,7 +17,6 @@
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import frc.robot.Constants.DeviceIDs;
-import frc.robot.subsystems.shooter.Shooter.ShooterSide;
 import frc.robot.subsystems.shooter.ShooterConstants.HoodConstants;
 import java.util.function.DoubleSupplier;
 
@@ -27,10 +26,10 @@ public class HoodIOSparkMax implements HoodIO {
   private final SparkClosedLoopController motorController;
   private final Debouncer connectedDebouncer = new Debouncer(0.5, DebounceType.kFalling);
 
-  public HoodIOSparkMax(ShooterSide side) {
+  public HoodIOSparkMax() {
     motor =
         new SparkMax(
-            side == ShooterSide.LEFT ? DeviceIDs.kLeftTurretHood : DeviceIDs.kRightTurretHood,
+            DeviceIDs.kTurretHood,
             MotorType.kBrushless);
     encoder = motor.getEncoder();
     motorController = motor.getClosedLoopController();
@@ -39,8 +38,6 @@ public HoodIOSparkMax(ShooterSide side) {
 
     config.idleMode(IdleMode.kCoast);
 
-    config.inverted(side == ShooterSide.RIGHT);
-
     config
         .encoder
         .positionConversionFactor(2 * Math.PI / HoodConstants.kGearRatio) // No absolute encoder...
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
index 65b1a31..7272395 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
@@ -15,7 +15,6 @@
 import frc.robot.RobotContainer;
 import frc.robot.RobotState;
 import frc.robot.RobotVisualizer;
-import frc.robot.subsystems.shooter.Shooter.ShooterSide;
 import frc.robot.subsystems.shooter.ShooterConstants.TurretConstants;
 import frc.robot.subsystems.shooter.turret.TurretIO.TurretIOOutputMode;
 import frc.robot.subsystems.shooter.turret.TurretIO.TurretIOOutputs;
@@ -24,8 +23,6 @@
 import org.littletonrobotics.junction.Logger;
 
 public class Turret extends FullSubsystem {
-  private final ShooterSide side;
-
   private final TurretIO io;
   private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged();
   private final TurretIOOutputs outputs = new TurretIOOutputs();
@@ -38,36 +35,31 @@ public class Turret extends FullSubsystem {
   private boolean isZeroed = true;
 
   /** Creates a new Turret. */
-  public Turret(ShooterSide side, TurretIO io) {
-    this.side = side;
+  public Turret(TurretIO io) {
     this.io = io;
   }
 
   @Override
   public void periodic() {
     io.updateInputs(inputs);
-    Logger.processInputs(("Turret/" + side.getName()), inputs);
+    Logger.processInputs("Turret", inputs);
 
     if (inputs.limitTriggered) {
       isZeroed = true;
     }
-
-    if (side == ShooterSide.LEFT) {
-      RobotVisualizer.getInstance().setLeftTurretAngle(Rotation2d.fromRadians(inputs.positionRad));
-    } else if (side == ShooterSide.RIGHT) {
-      RobotVisualizer.getInstance().setRightTurretAngle(Rotation2d.fromRadians(inputs.positionRad));
-    }
+    
+    RobotVisualizer.getInstance().setTurretAzimuthAngle(Rotation2d.fromRadians(inputs.positionRad));
   }
 
   @Override
   public void periodicAfterScheduler() {
     io.applyOutputs(outputs);
     Logger.recordOutput("Turret/Mode", outputs.mode.toString());
-    Logger.recordOutput(("Turret/" + side.getName() + "/TargetAngle"), targetAngle);
+    Logger.recordOutput(("Turret/TargetAngle"), targetAngle);
     Logger.recordOutput(
-        ("Turret/" + side.getName() + "/TargetAngleDegrees"), targetAngle.getDegrees());
+        ("Turret/TargetAngleDegrees"), targetAngle.getDegrees());
     Logger.recordOutput(
-        ("Turret/" + side.getName() + "/TargetOffsetDegrees"),
+        ("Turret/TargetOffsetDegrees"),
         targetAngle.minus(Rotation2d.fromRadians(inputs.positionRad)).getDegrees());
   }
 
@@ -155,8 +147,4 @@ public boolean atGoal() {
   public boolean isZeroed() {
     return isZeroed;
   }
-
-  public ShooterSide getSide() {
-    return this.side;
-  }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
index af4875d..6a48012 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
@@ -4,6 +4,7 @@
 import static frc.robot.util.SparkUtil.sparkStickyFault;
 import static frc.robot.util.SparkUtil.tryUntilOk;
 
+import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.PersistMode;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.ResetMode;
@@ -19,39 +20,27 @@
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import frc.robot.Constants.DeviceIDs;
-import frc.robot.subsystems.shooter.Shooter.ShooterSide;
 import frc.robot.subsystems.shooter.ShooterConstants.TurretConstants;
 import java.util.function.DoubleSupplier;
 
 public class TurretIOSparkMax implements TurretIO {
   private final SparkMax motor;
-  private final RelativeEncoder encoder;
+  private final AbsoluteEncoder encoder;
   private final SparkClosedLoopController motorController;
 
   private final Debouncer connectedDebouncer = new Debouncer(0.5, DebounceType.kFalling);
 
-  public TurretIOSparkMax(ShooterSide side) {
+  public TurretIOSparkMax() {
     motor =
         new SparkMax(
-            side == ShooterSide.LEFT ? DeviceIDs.kLeftTurretAzimuth : DeviceIDs.kRightTurretAzimuth,
+            DeviceIDs.kTurretAzimuth,
             MotorType.kBrushless);
-    encoder = motor.getEncoder();
+    encoder = motor.getAbsoluteEncoder();
     motorController = motor.getClosedLoopController();
 
-    encoder.setPosition(0);
-
     SparkMaxConfig config = new SparkMaxConfig();
 
     config.idleMode(IdleMode.kCoast);
-    // TODO: Tune
-    config.inverted(side == ShooterSide.RIGHT);
-    // .smartCurrentLimit(30);
-
-    config
-        .encoder
-        .positionConversionFactor(
-            2 * Math.PI / TurretConstants.kGearRatio) // No absolute encoder...
-        .velocityConversionFactor(2 * Math.PI / TurretConstants.kGearRatio / 60.0);
 
     config.closedLoop.positionWrappingEnabled(true).feedbackSensor(FeedbackSensor.kPrimaryEncoder);
 
@@ -69,7 +58,6 @@ public TurretIOSparkMax(ShooterSide side) {
         () ->
             motor.configure(
                 config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters));
-    tryUntilOk(motor, 5, () -> encoder.setPosition(0));
   }
 
   @Override

From fee7a702301f2230487d7fee2c0665cf87589e77 Mon Sep 17 00:00:00 2001
From: Maxwell Morgan 
Date: Mon, 16 Mar 2026 17:10:01 -0400
Subject: [PATCH 52/61] Add indexer subsystem

---
 src/main/java/frc/robot/Constants.java        |  6 +-
 src/main/java/frc/robot/RobotContainer.java   | 18 ++--
 src/main/java/frc/robot/RobotVisualizer.java  |  6 +-
 .../java/frc/robot/control/ZoneControls.java  | 14 ---
 .../frc/robot/subsystems/indexer/Indexer.java | 62 ++++++++++++
 .../subsystems/indexer/IndexerConstants.java  |  6 ++
 .../robot/subsystems/indexer/IndexerIO.java   | 26 +++++
 .../subsystems/indexer/IndexerIOSim.java      | 28 ++++++
 .../subsystems/indexer/IndexerIOTalonFX.java  | 95 +++++++++++++++++++
 .../frc/robot/subsystems/shooter/Shooter.java |  2 -
 .../shooter/TrajectoryCalculator.java         |  9 +-
 .../shooter/flywheel/FlywheelIOTalonFX.java   |  6 +-
 .../shooter/hood/HoodIOSparkMax.java          |  5 +-
 .../subsystems/shooter/turret/Turret.java     |  5 +-
 .../shooter/turret/TurretIOSparkMax.java      |  6 +-
 15 files changed, 242 insertions(+), 52 deletions(-)
 create mode 100644 src/main/java/frc/robot/subsystems/indexer/Indexer.java
 create mode 100644 src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java
 create mode 100644 src/main/java/frc/robot/subsystems/indexer/IndexerIO.java
 create mode 100644 src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java
 create mode 100644 src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 1646530..4a2bdf1 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -78,10 +78,12 @@ public static final class DeviceIDs {
         DriveConstants.TunerConstants.BackRight.EncoderId; // 22
 
     public static final int kTurretFlywheel = 12;
-    public static final int kTurretHood = 15;
+    public static final int kTurretHood = 13;
     public static final int kTurretAzimuth = 14;
 
-    public static final int kGuts = 16;
+    public static final int kGuts = 15;
+
+    public static final int kIndexer = 16;
 
     public static final int kIntakeDrive = 17;
     public static final int kIntakePivot = 18;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 18af04d..54c37af 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -16,20 +16,17 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.RobotState.OdometryObservation;
 import frc.robot.RobotState.VisionMeasurement;
+import frc.robot.control.Configurable;
 import frc.robot.control.DriverController;
+import frc.robot.control.ZoneControls;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.GyroIOPigeon2;
 import frc.robot.subsystems.drive.ModuleIO;
-import frc.robot.subsystems.intake.Intake;
-import frc.robot.subsystems.shooter.Shooter;
-import frc.robot.subsystems.shooter.hood.Hood;
-import frc.robot.subsystems.shooter.hood.HoodIOSparkMax;
-import frc.robot.subsystems.shooter.turret.Turret;
-import frc.robot.subsystems.shooter.turret.TurretIOSparkMax;
 import frc.robot.subsystems.vision.CameraIOLimelight;
 import frc.robot.subsystems.vision.Vision;
 import frc.robot.subsystems.vision.Vision.VisionConsumer;
 import frc.robot.util.GeomUtil;
+import java.util.List;
 import java.util.function.Supplier;
 
 public class RobotContainer {
@@ -51,12 +48,11 @@ public RobotContainer() {
     field2d.setRobotPose(RobotState.getInstance().getEstimatedPose());
     switch (Constants.kCurrentMode) {
       case REAL:
-        
+
       case SIM:
 
       case REPLAY:
       default:
-        
     }
     vision =
         new Vision(
@@ -82,6 +78,12 @@ public void accept(
             new ModuleIO() {},
             new ModuleIO() {},
             new ModuleIO() {});
+
+    configureBindings();
+  }
+
+  private void configureBindings() {
+    List.of(new ZoneControls()).forEach(Configurable::configure);
   }
 
   public void robotPeriodic() {
diff --git a/src/main/java/frc/robot/RobotVisualizer.java b/src/main/java/frc/robot/RobotVisualizer.java
index 9afe1aa..a4c0481 100644
--- a/src/main/java/frc/robot/RobotVisualizer.java
+++ b/src/main/java/frc/robot/RobotVisualizer.java
@@ -41,11 +41,9 @@ public void log(String key) {
     Pose3d hoodPose =
         turretPose.transformBy(
             new Transform3d(
-                HoodConstants.kTurretToHood.getTranslation(),
-                new Rotation3d(0.0, hoodAngle, 0.0)));
+                HoodConstants.kTurretToHood.getTranslation(), new Rotation3d(0.0, hoodAngle, 0.0)));
 
-    Logger.recordOutput(
-        key + "/Components", turretPose, hoodPose);
+    Logger.recordOutput(key + "/Components", turretPose, hoodPose);
   }
 
   /**
diff --git a/src/main/java/frc/robot/control/ZoneControls.java b/src/main/java/frc/robot/control/ZoneControls.java
index 3a16408..c7e4348 100644
--- a/src/main/java/frc/robot/control/ZoneControls.java
+++ b/src/main/java/frc/robot/control/ZoneControls.java
@@ -1,22 +1,8 @@
 package frc.robot.control;
 
-import org.littletonrobotics.junction.Logger;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import frc.robot.util.FieldConstants;
-import frc.robot.util.Zone;
-
 public class ZoneControls implements Configurable {
 
-  private Zone leftTrenchZone = new Zone.RectangleZone(FieldConstants.LeftTrench.openingTopLeft.toTranslation2d(),
-      FieldConstants.LeftTrench.openingTopRight.toTranslation2d()
-          .plus(new Translation2d(FieldConstants.LeftTrench.depth, 0)));
-
   @Override
   public void configure() {
-    Logger.recordOutput("TrenchZoneLeft",
-        new Translation2d[] { FieldConstants.LeftTrench.openingTopLeft.toTranslation2d(),
-            FieldConstants.LeftTrench.openingTopRight.toTranslation2d()
-                .plus(new Translation2d(FieldConstants.LeftTrench.depth, 0)) });
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java
new file mode 100644
index 0000000..ddec03d
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java
@@ -0,0 +1,62 @@
+// 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.indexer;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class Indexer extends SubsystemBase {
+  private final IndexerIO io;
+  private final IndexerIOInputsAutoLogged inputs = new IndexerIOInputsAutoLogged();
+
+  /** Creates a new Indexer. */
+  public Indexer(IndexerIO io) {
+    this.io = io;
+  }
+
+  @Override
+  public void periodic() {
+    io.updateInputs(inputs);
+    Logger.processInputs("Indexer", inputs);
+  }
+
+  public Command index() {
+    return Commands.startEnd(
+        () -> {
+          io.setThroatOpenLoop(IndexerConstants.kThroatMotorSpeed);
+          io.setToungeOpenLoop(IndexerConstants.kToungeMotorSpeed);
+        },
+        () -> {
+          io.stop();
+        },
+        this);
+  }
+
+  public Command indexReverse() {
+    return Commands.startEnd(
+        () -> {
+          io.setThroatOpenLoop(-IndexerConstants.kThroatMotorSpeed);
+          io.setToungeOpenLoop(-IndexerConstants.kToungeMotorSpeed);
+        },
+        () -> {
+          io.stop();
+        },
+        this);
+  }
+
+  public void setThroatOpenLoop(double output) {
+    io.setThroatOpenLoop(output);
+  }
+
+  public void setToungeOpenLoop(double output) {
+    io.setToungeOpenLoop(output);
+  }
+
+  public void stop() {
+    io.stop();
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java
new file mode 100644
index 0000000..211a8ca
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java
@@ -0,0 +1,6 @@
+package frc.robot.subsystems.indexer;
+
+public final class IndexerConstants {
+  public static final double kThroatMotorSpeed = 0.5;
+  public static final double kToungeMotorSpeed = 0.5;
+}
diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java
new file mode 100644
index 0000000..2c720f9
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java
@@ -0,0 +1,26 @@
+package frc.robot.subsystems.indexer;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IndexerIO {
+  @AutoLog
+  public static class IndexerIOInputs {
+    public boolean throatConnected = false;
+    public double throatVelocityRadPerSec = 0.0;
+    public double throatAppliedVolts = 0.0;
+    public double throatCurrentDrawAmps = 0.0;
+
+    public boolean toungeConnected = false;
+    public double toungeVelocityRadPerSec = 0.0;
+    public double toungeAppliedVolts = 0.0;
+    public double toungeCurrentDrawAmps = 0.0;
+  }
+
+  default void updateInputs(IndexerIOInputs inputs) {}
+
+  default void setThroatOpenLoop(double output) {}
+
+  default void setToungeOpenLoop(double output) {}
+
+  default void stop() {}
+}
diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java
new file mode 100644
index 0000000..dc54eb8
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java
@@ -0,0 +1,28 @@
+package frc.robot.subsystems.indexer;
+
+public class IndexerIOSim implements IndexerIO {
+
+  @Override
+  public void updateInputs(IndexerIOInputs inputs) {
+    // TODO Auto-generated method stub
+    IndexerIO.super.updateInputs(inputs);
+  }
+
+  @Override
+  public void setThroatOpenLoop(double output) {
+    // TODO Auto-generated method stub
+    IndexerIO.super.setThroatOpenLoop(output);
+  }
+
+  @Override
+  public void setToungeOpenLoop(double output) {
+    // TODO Auto-generated method stub
+    IndexerIO.super.setThroatOpenLoop(output);
+  }
+
+  @Override
+  public void stop() {
+    // TODO Auto-generated method stub
+    IndexerIO.super.stop();
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java
new file mode 100644
index 0000000..69ef055
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java
@@ -0,0 +1,95 @@
+package frc.robot.subsystems.indexer;
+
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static frc.robot.util.PhoenixUtil.tryUntilOk;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.ParentDevice;
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.units.measure.AngularAcceleration;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Voltage;
+import frc.robot.Constants.DeviceIDs;
+
+public class IndexerIOTalonFX implements IndexerIO {
+  private final TalonFX throatMotor = new TalonFX(DeviceIDs.kIndexer);
+  private final TalonFX toungeMotor = new TalonFX(DeviceIDs.kIndexer);
+
+  private final StatusSignal throatVelocity;
+  private final StatusSignal throatAcceleration;
+  private final StatusSignal throatVoltage;
+  private final StatusSignal throatCurrent;
+
+  private final StatusSignal toungeVelocity;
+  private final StatusSignal toungeAcceleration;
+  private final StatusSignal toungeVoltage;
+  private final StatusSignal toungeCurrent;
+
+  public IndexerIOTalonFX() {
+    TalonFXConfiguration throatMotorConfig = new TalonFXConfiguration();
+    TalonFXConfiguration toungeMotorConfig = new TalonFXConfiguration();
+
+    throatVelocity = throatMotor.getVelocity();
+    throatAcceleration = throatMotor.getAcceleration();
+    throatVoltage = throatMotor.getMotorVoltage();
+    throatCurrent = throatMotor.getSupplyCurrent();
+
+    toungeVelocity = toungeMotor.getVelocity();
+    toungeAcceleration = toungeMotor.getAcceleration();
+    toungeVoltage = toungeMotor.getMotorVoltage();
+    toungeCurrent = toungeMotor.getSupplyCurrent();
+
+    tryUntilOk(5, () -> throatMotor.getConfigurator().apply(throatMotorConfig));
+    tryUntilOk(5, () -> toungeMotor.getConfigurator().apply(toungeMotorConfig));
+
+    BaseStatusSignal.setUpdateFrequencyForAll(
+        50,
+        throatVelocity,
+        throatAcceleration,
+        throatVoltage,
+        throatCurrent,
+        toungeVelocity,
+        toungeAcceleration,
+        toungeCurrent);
+    ParentDevice.optimizeBusUtilizationForAll(throatMotor, toungeMotor);
+  }
+
+  @Override
+  public void updateInputs(IndexerIOInputs inputs) {
+    inputs.throatConnected =
+        BaseStatusSignal.refreshAll(
+                throatVelocity, throatAcceleration, throatVoltage, throatCurrent)
+            .isOK();
+    inputs.throatVelocityRadPerSec = throatVelocity.getValue().in(RadiansPerSecond);
+    inputs.throatAppliedVolts = throatAcceleration.getValueAsDouble();
+    inputs.throatCurrentDrawAmps = throatCurrent.getValueAsDouble();
+
+    inputs.toungeConnected =
+        BaseStatusSignal.refreshAll(
+                toungeVelocity, toungeAcceleration, toungeVoltage, toungeCurrent)
+            .isOK();
+    inputs.toungeVelocityRadPerSec = toungeVelocity.getValue().in(RadiansPerSecond);
+    inputs.toungeAppliedVolts = toungeAcceleration.getValueAsDouble();
+    inputs.toungeCurrentDrawAmps = toungeCurrent.getValueAsDouble();
+  }
+
+  @Override
+  public void setThroatOpenLoop(double output) {
+    throatMotor.set(MathUtil.clamp(output, -1.0, 1.0));
+  }
+
+  @Override
+  public void setToungeOpenLoop(double output) {
+    toungeMotor.set(MathUtil.clamp(output, -1.0, 1.0));
+  }
+
+  @Override
+  public void stop() {
+    throatMotor.stopMotor();
+    toungeMotor.stopMotor();
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 721838a..31c5ec4 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -16,8 +16,6 @@
 import frc.robot.subsystems.shooter.hood.HoodIO;
 import frc.robot.subsystems.shooter.turret.Turret;
 import frc.robot.subsystems.shooter.turret.TurretIO;
-import frc.robot.util.AllianceFlipUtil;
-import frc.robot.util.FieldConstants.Hub;
 import java.util.function.Supplier;
 
 public class Shooter extends SubsystemBase {
diff --git a/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java
index 75086f3..aad941f 100644
--- a/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java
+++ b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java
@@ -72,7 +72,8 @@ private static RobotStateData getCompensatedRobotState() {
   }
 
   /** Calculate shooter command for a specific side using pre-computed robot state. */
-  private static ShooterCommand calculateWithState(Translation2d targetLocation, RobotStateData state) {
+  private static ShooterCommand calculateWithState(
+      Translation2d targetLocation, RobotStateData state) {
 
     // 2. Identify Turret Offset and Position
     Transform3d robotToTurret = TurretConstants.kRobotToTurret;
@@ -117,14 +118,12 @@ private static ShooterCommand calculateWithState(Translation2d targetLocation, R
     Pose2d lookaheadRobotPose =
         lookaheadTurretPose.transformBy(GeomUtil.toTransform2d(robotToTurret).inverse());
 
-    Logger.recordOutput(
-        "LaunchCalculator/LookaheadRobotPose", lookaheadRobotPose);
+    Logger.recordOutput("LaunchCalculator/LookaheadRobotPose", lookaheadRobotPose);
     Logger.recordOutput(
         "LaunchCalculator/ShotVector",
         new Pose2d(lookaheadRobotPose.getTranslation(), turretAngleField));
     Logger.recordOutput("LaunchCalculator/Distance", lookaheadDistance);
-    Logger.recordOutput(
-        "LaunchCalculator/DistanceClamped", clampedFinalDistance);
+    Logger.recordOutput("LaunchCalculator/DistanceClamped", clampedFinalDistance);
     Logger.recordOutput(
         "LaunchCalculator/IsInRange",
         lookaheadDistance >= MIN_SHOOTING_DISTANCE && lookaheadDistance <= MAX_SHOOTING_DISTANCE);
diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java
index 3d085db..e7fe921 100644
--- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java
@@ -5,11 +5,9 @@
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
-import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.InvertedValue;
 import edu.wpi.first.units.measure.AngularAcceleration;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
@@ -29,9 +27,7 @@ public class FlywheelIOTalonFX implements FlywheelIO {
   private final VelocityVoltage velocityRequest = new VelocityVoltage(0).withSlot(0);
 
   public FlywheelIOTalonFX() {
-    motor =
-        new TalonFX(
-            DeviceIDs.kTurretFlywheel);
+    motor = new TalonFX(DeviceIDs.kTurretFlywheel);
     motorConfig =
         new TalonFXConfiguration()
             .withSlot0(FlywheelConstants.kGains)
diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
index c9a8420..e8ec6c6 100644
--- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
@@ -27,10 +27,7 @@ public class HoodIOSparkMax implements HoodIO {
   private final Debouncer connectedDebouncer = new Debouncer(0.5, DebounceType.kFalling);
 
   public HoodIOSparkMax() {
-    motor =
-        new SparkMax(
-            DeviceIDs.kTurretHood,
-            MotorType.kBrushless);
+    motor = new SparkMax(DeviceIDs.kTurretHood, MotorType.kBrushless);
     encoder = motor.getEncoder();
     motorController = motor.getClosedLoopController();
 
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
index 7272395..d1f1e33 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
@@ -47,7 +47,7 @@ public void periodic() {
     if (inputs.limitTriggered) {
       isZeroed = true;
     }
-    
+
     RobotVisualizer.getInstance().setTurretAzimuthAngle(Rotation2d.fromRadians(inputs.positionRad));
   }
 
@@ -56,8 +56,7 @@ public void periodicAfterScheduler() {
     io.applyOutputs(outputs);
     Logger.recordOutput("Turret/Mode", outputs.mode.toString());
     Logger.recordOutput(("Turret/TargetAngle"), targetAngle);
-    Logger.recordOutput(
-        ("Turret/TargetAngleDegrees"), targetAngle.getDegrees());
+    Logger.recordOutput(("Turret/TargetAngleDegrees"), targetAngle.getDegrees());
     Logger.recordOutput(
         ("Turret/TargetOffsetDegrees"),
         targetAngle.minus(Rotation2d.fromRadians(inputs.positionRad)).getDegrees());
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
index 6a48012..88467ad 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
@@ -6,7 +6,6 @@
 
 import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.PersistMode;
-import com.revrobotics.RelativeEncoder;
 import com.revrobotics.ResetMode;
 import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.FeedbackSensor;
@@ -31,10 +30,7 @@ public class TurretIOSparkMax implements TurretIO {
   private final Debouncer connectedDebouncer = new Debouncer(0.5, DebounceType.kFalling);
 
   public TurretIOSparkMax() {
-    motor =
-        new SparkMax(
-            DeviceIDs.kTurretAzimuth,
-            MotorType.kBrushless);
+    motor = new SparkMax(DeviceIDs.kTurretAzimuth, MotorType.kBrushless);
     encoder = motor.getAbsoluteEncoder();
     motorController = motor.getClosedLoopController();
 

From ce8ea3ec995d3d13327806481a7ba1514a4769e6 Mon Sep 17 00:00:00 2001
From: Maxwell Morgan 
Date: Mon, 16 Mar 2026 22:05:33 -0400
Subject: [PATCH 53/61] Update subsystems for new robot

---
 src/main/java/frc/robot/Constants.java        |   3 +-
 .../frc/robot/control/DriverControls.java     |  53 ++++----
 .../java/frc/robot/control/ZoneControls.java  |   3 +-
 .../java/frc/robot/subsystems/guts/Guts.java  |  25 +---
 .../frc/robot/subsystems/guts/GutsIO.java     |   3 +-
 .../frc/robot/subsystems/guts/GutsIOSim.java  |   3 +-
 .../robot/subsystems/guts/GutsIOSparkMax.java |  44 -------
 .../robot/subsystems/guts/GutsIOTalonFX.java  |  57 +++++++++
 .../subsystems/indexer/IndexerIOTalonFX.java  |  26 +---
 .../frc/robot/subsystems/intake/Intake.java   |  12 +-
 .../subsystems/intake/IntakeConstants.java    |   8 ++
 .../frc/robot/subsystems/intake/IntakeIO.java |  27 ++--
 .../subsystems/intake/IntakeIOHardware.java   |  58 ---------
 .../robot/subsystems/intake/IntakeIOSim.java  |  88 ++++++-------
 .../subsystems/intake/IntakeIOTalonFX.java    | 118 ++++++++++++++++++
 .../subsystems/shooter/ShooterConstants.java  |   9 +-
 .../shooter/turret/TurretIOSparkMax.java      |  14 ++-
 .../frc/robot/subsystems/vision/Vision.java   |   4 -
 18 files changed, 300 insertions(+), 255 deletions(-)
 delete mode 100644 src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java
 create mode 100644 src/main/java/frc/robot/subsystems/guts/GutsIOTalonFX.java
 delete mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java
 create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 4a2bdf1..d867957 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -86,6 +86,7 @@ public static final class DeviceIDs {
     public static final int kIndexer = 16;
 
     public static final int kIntakeDrive = 17;
-    public static final int kIntakePivot = 18;
+    public static final int kLeftIntakePivot = 18;
+    public static final int kRightIntakePivot = 19;
   }
 }
diff --git a/src/main/java/frc/robot/control/DriverControls.java b/src/main/java/frc/robot/control/DriverControls.java
index 87ea624..4229f22 100644
--- a/src/main/java/frc/robot/control/DriverControls.java
+++ b/src/main/java/frc/robot/control/DriverControls.java
@@ -5,6 +5,7 @@
 import frc.robot.commands.DriveCommands;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.guts.Guts;
+import frc.robot.subsystems.indexer.Indexer;
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.util.Direction;
@@ -13,29 +14,26 @@ public class DriverControls implements Configurable {
   private final DriverController driver;
   private final DriverController operator;
   private final Drive drive;
-  private final Shooter leftShooter;
-  private final Shooter rightShooter;
-  private final Guts leftGuts;
-  private final Guts rightGuts;
+  private final Shooter shooter;
+  private final Guts guts;
   private final Intake intake;
+  private final Indexer indexer;
 
   public DriverControls(
       DriverController driver,
       DriverController operator,
       Drive drive,
-      Shooter leftShooter,
-      Shooter rightShooter,
-      Guts leftGuts,
-      Guts rightGuts,
-      Intake intake) {
+      Shooter shooter,
+      Guts guts,
+      Intake intake,
+      Indexer indexer) {
     this.driver = driver;
     this.operator = operator;
     this.drive = drive;
-    this.leftShooter = leftShooter;
-    this.rightShooter = rightShooter;
-    this.leftGuts = leftGuts;
-    this.rightGuts = rightGuts;
+    this.shooter = shooter;
+    this.guts = guts;
     this.intake = intake;
+    this.indexer = indexer;
   }
 
   @Override
@@ -81,25 +79,22 @@ private void configureOperatorControls() {
     operator
         .rightBumper()
         .whileTrue(
-            rightShooter
-                .setFlywheelVelocity(8500)
-                .alongWith(leftShooter.setFlywheelVelocity(-8500)));
+            shooter
+                .setFlywheelVelocity(8500));
 
     operator
         .rightTrigger()
-        .whileTrue(leftGuts.runGutForward().alongWith(rightGuts.runGutForward()));
+        .whileTrue(guts.runGutForward());
 
     operator
         .dPadUp()
         .whileTrue(
             new StartEndCommand(
                 () -> {
-                  leftShooter.setHoodOpenLoop(0.05);
-                  rightShooter.setHoodOpenLoop(0.05);
+                  shooter.setHoodOpenLoop(0.05);
                 },
                 () -> {
-                  leftShooter.setHoodOpenLoop(0);
-                  rightShooter.setHoodOpenLoop(0);
+                  shooter.setHoodOpenLoop(0);
                 }));
 
     operator
@@ -107,26 +102,22 @@ private void configureOperatorControls() {
         .whileTrue(
             new StartEndCommand(
                 () -> {
-                  leftShooter.setHoodOpenLoop(-0.05);
-                  rightShooter.setHoodOpenLoop(-0.05);
+                  shooter.setHoodOpenLoop(-0.05);
                 },
                 () -> {
-                  leftShooter.setHoodOpenLoop(0);
-                  rightShooter.setHoodOpenLoop(0);
+                  shooter.setHoodOpenLoop(0);
                 }));
 
     operator.leftTrigger().whileTrue(intake.outtake());
     operator.aCross().whileTrue(intake.outtake());
-    operator.xSquare().whileTrue(intake.deploy());
-    operator.yTriangle().whileTrue(intake.retract());
+    operator.xSquare().whileTrue(intake.deployOpenLoop());
+    operator.yTriangle().whileTrue(intake.retractOpenLoop());
     operator
         .bCircle()
         .whileTrue(
-            rightShooter
+            shooter
                 .setFlywheelVelocity(2000)
                 .alongWith(
-                    leftShooter.setFlywheelVelocity(-2000),
-                    rightGuts.runGutForward(),
-                    leftGuts.runGutForward()));
+                    guts.runGutForward()));
   }
 }
diff --git a/src/main/java/frc/robot/control/ZoneControls.java b/src/main/java/frc/robot/control/ZoneControls.java
index c7e4348..6927d80 100644
--- a/src/main/java/frc/robot/control/ZoneControls.java
+++ b/src/main/java/frc/robot/control/ZoneControls.java
@@ -3,6 +3,5 @@
 public class ZoneControls implements Configurable {
 
   @Override
-  public void configure() {
-  }
+  public void configure() {}
 }
diff --git a/src/main/java/frc/robot/subsystems/guts/Guts.java b/src/main/java/frc/robot/subsystems/guts/Guts.java
index 2d84cfe..17a0764 100644
--- a/src/main/java/frc/robot/subsystems/guts/Guts.java
+++ b/src/main/java/frc/robot/subsystems/guts/Guts.java
@@ -17,47 +17,30 @@
  */
 public class Guts extends SubsystemBase {
 
-  private final GutSide side;
   private final GutsIO io;
   private GutsIOInputsAutoLogged inputs = new GutsIOInputsAutoLogged();
   private final double speed;
 
   /** Creates a new Guts. */
-  public Guts(GutSide side, GutsIO io) {
+  public Guts(GutsIO io) {
     this.io = io;
-    this.side = side;
     speed = GutsConstants.kGutMotorSpeed;
   }
 
   /** Runs the gut motor forward at 0.5 speed, then stops it when finished. */
   public Command runGutForward() {
-    return Commands.runEnd(() -> io.setGutMotorSpeed(speed), () -> io.setGutMotorSpeed(0), this);
+    return Commands.runEnd(() -> io.setOpenLoop(speed), () -> io.setOpenLoop(0), this);
   }
 
   /** Runs the gut motor backward at 0.5 speed, then stops it when finished. */
   public Command runGutBackward() {
-    return Commands.runEnd(() -> io.setGutMotorSpeed(-speed), () -> io.setGutMotorSpeed(0), this);
+    return Commands.runEnd(() -> io.setOpenLoop(-speed), () -> io.setOpenLoop(0), this);
   }
 
   @Override
   public void periodic() {
     io.updateInputs(inputs);
-    Logger.processInputs("Guts/" + side.getName(), inputs);
+    Logger.processInputs("Guts", inputs);
     // This method will be called once per scheduler run
   }
-
-  public enum GutSide {
-    LEFT("Left"),
-    RIGHT("Right");
-
-    private final String name;
-
-    private GutSide(String name) {
-      this.name = name;
-    }
-
-    public String getName() {
-      return name;
-    }
-  }
 }
diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIO.java b/src/main/java/frc/robot/subsystems/guts/GutsIO.java
index d6301ca..820dcfe 100644
--- a/src/main/java/frc/robot/subsystems/guts/GutsIO.java
+++ b/src/main/java/frc/robot/subsystems/guts/GutsIO.java
@@ -17,11 +17,10 @@ default void updateInputs(GutsIOInputs inputs) {}
   @AutoLog
   public static class GutsIOInputs {
     public double velocityRadPerSec = 0.0;
-    public double positionRad = 0.0;
     public double appliedVolts = 0.0;
     public double currentDrawAmps = 0.0;
   }
 
   /** Sets the gut motor to a specific speed ranging from -1.0 to 1.0 */
-  default void setGutMotorSpeed(double speed) {}
+  default void setOpenLoop(double speed) {}
 }
diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIOSim.java b/src/main/java/frc/robot/subsystems/guts/GutsIOSim.java
index 65cd935..e3f6f2b 100644
--- a/src/main/java/frc/robot/subsystems/guts/GutsIOSim.java
+++ b/src/main/java/frc/robot/subsystems/guts/GutsIOSim.java
@@ -28,12 +28,11 @@ public void updateInputs(GutsIOInputs inputs) {
     sim.setInputVoltage(appliedVolts);
     sim.update(0.02);
 
-    inputs.positionRad = sim.getAngularPositionRotations();
     inputs.velocityRadPerSec = sim.getAngularVelocityRPM();
   }
 
   @Override
-  public void setGutMotorSpeed(double speed) {
+  public void setOpenLoop(double speed) {
     appliedVolts = 12 * speed;
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java b/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java
deleted file mode 100644
index 31e203f..0000000
--- a/src/main/java/frc/robot/subsystems/guts/GutsIOSparkMax.java
+++ /dev/null
@@ -1,44 +0,0 @@
-package frc.robot.subsystems.guts;
-
-import com.revrobotics.PersistMode;
-import com.revrobotics.RelativeEncoder;
-import com.revrobotics.ResetMode;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.config.SparkMaxConfig;
-import edu.wpi.first.math.util.Units;
-
-/**
- * This class contains all of the physical objects: one motor and its corresponding encoder. It also
- * implements the default methods specified in the IO interface to set the speed of the physical
- * motor and update the input values using the encoders.
- *
- * @author Ryan Hefferon
- */
-public class GutsIOSparkMax implements GutsIO {
-
-  private final SparkMax gutMotor;
-  private final RelativeEncoder gutEncoder;
-  private final SparkMaxConfig gutMotorConfig;
-
-  public GutsIOSparkMax(int motorID) {
-    gutMotor = new SparkMax(motorID, MotorType.kBrushless);
-    gutEncoder = gutMotor.getEncoder();
-    gutMotorConfig = new SparkMaxConfig();
-    gutMotor.configure(
-        gutMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
-  }
-
-  @Override
-  public void setGutMotorSpeed(double speed) {
-    gutMotor.set(speed);
-  }
-
-  @Override
-  public void updateInputs(GutsIOInputs inputs) {
-    inputs.velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(gutEncoder.getVelocity());
-    inputs.positionRad = Units.rotationsToRadians(gutEncoder.getPosition());
-    inputs.appliedVolts = gutMotor.getAppliedOutput();
-    inputs.currentDrawAmps = gutMotor.getOutputCurrent();
-  }
-}
diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIOTalonFX.java b/src/main/java/frc/robot/subsystems/guts/GutsIOTalonFX.java
new file mode 100644
index 0000000..a7c1e72
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/guts/GutsIOTalonFX.java
@@ -0,0 +1,57 @@
+package frc.robot.subsystems.guts;
+
+import static frc.robot.util.PhoenixUtil.tryUntilOk;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.revrobotics.PersistMode;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Voltage;
+import frc.robot.Constants.DeviceIDs;
+
+/**
+ * This class contains all of the physical objects: one motor and its corresponding encoder. It also
+ * implements the default methods specified in the IO interface to set the speed of the physical
+ * motor and update the input values using the encoders.
+ *
+ * @author Ryan Hefferon
+ */
+public class GutsIOTalonFX implements GutsIO {
+
+  private final TalonFX motor = new TalonFX(DeviceIDs.kGuts);
+  
+  private final StatusSignal velocitySignal;
+  private final StatusSignal voltageSignal;
+  private final StatusSignal currentSignal;
+
+  public GutsIOTalonFX() {
+    TalonFXConfiguration motorConfig = new TalonFXConfiguration();
+
+    velocitySignal = motor.getVelocity();
+    voltageSignal = motor.getMotorVoltage();
+    currentSignal = motor.getSupplyCurrent();
+    
+    tryUntilOk(5, () -> motor.getConfigurator().apply(motorConfig));
+
+    BaseStatusSignal.setUpdateFrequencyForAll(50, velocitySignal, voltageSignal, currentSignal);
+    motor.optimizeBusUtilization();
+  }
+
+  @Override
+  public void setOpenLoop(double speed) {
+  }
+
+  @Override
+  public void updateInputs(GutsIOInputs inputs) {
+
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java
index 69ef055..84b71bb 100644
--- a/src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java
@@ -9,7 +9,6 @@
 import com.ctre.phoenix6.hardware.ParentDevice;
 import com.ctre.phoenix6.hardware.TalonFX;
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.units.measure.AngularAcceleration;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.units.measure.Voltage;
@@ -20,12 +19,10 @@ public class IndexerIOTalonFX implements IndexerIO {
   private final TalonFX toungeMotor = new TalonFX(DeviceIDs.kIndexer);
 
   private final StatusSignal throatVelocity;
-  private final StatusSignal throatAcceleration;
   private final StatusSignal throatVoltage;
   private final StatusSignal throatCurrent;
 
   private final StatusSignal toungeVelocity;
-  private final StatusSignal toungeAcceleration;
   private final StatusSignal toungeVoltage;
   private final StatusSignal toungeCurrent;
 
@@ -34,12 +31,10 @@ public IndexerIOTalonFX() {
     TalonFXConfiguration toungeMotorConfig = new TalonFXConfiguration();
 
     throatVelocity = throatMotor.getVelocity();
-    throatAcceleration = throatMotor.getAcceleration();
     throatVoltage = throatMotor.getMotorVoltage();
     throatCurrent = throatMotor.getSupplyCurrent();
 
     toungeVelocity = toungeMotor.getVelocity();
-    toungeAcceleration = toungeMotor.getAcceleration();
     toungeVoltage = toungeMotor.getMotorVoltage();
     toungeCurrent = toungeMotor.getSupplyCurrent();
 
@@ -47,33 +42,22 @@ public IndexerIOTalonFX() {
     tryUntilOk(5, () -> toungeMotor.getConfigurator().apply(toungeMotorConfig));
 
     BaseStatusSignal.setUpdateFrequencyForAll(
-        50,
-        throatVelocity,
-        throatAcceleration,
-        throatVoltage,
-        throatCurrent,
-        toungeVelocity,
-        toungeAcceleration,
-        toungeCurrent);
+        50, throatVelocity, throatVoltage, throatCurrent, toungeVelocity, toungeCurrent);
     ParentDevice.optimizeBusUtilizationForAll(throatMotor, toungeMotor);
   }
 
   @Override
   public void updateInputs(IndexerIOInputs inputs) {
     inputs.throatConnected =
-        BaseStatusSignal.refreshAll(
-                throatVelocity, throatAcceleration, throatVoltage, throatCurrent)
-            .isOK();
+        BaseStatusSignal.refreshAll(throatVelocity, throatVoltage, throatCurrent).isOK();
     inputs.throatVelocityRadPerSec = throatVelocity.getValue().in(RadiansPerSecond);
-    inputs.throatAppliedVolts = throatAcceleration.getValueAsDouble();
+    inputs.throatAppliedVolts = throatVoltage.getValueAsDouble();
     inputs.throatCurrentDrawAmps = throatCurrent.getValueAsDouble();
 
     inputs.toungeConnected =
-        BaseStatusSignal.refreshAll(
-                toungeVelocity, toungeAcceleration, toungeVoltage, toungeCurrent)
-            .isOK();
+        BaseStatusSignal.refreshAll(toungeVelocity, toungeVoltage, toungeCurrent).isOK();
     inputs.toungeVelocityRadPerSec = toungeVelocity.getValue().in(RadiansPerSecond);
-    inputs.toungeAppliedVolts = toungeAcceleration.getValueAsDouble();
+    inputs.toungeAppliedVolts = toungeVoltage.getValueAsDouble();
     inputs.toungeCurrentDrawAmps = toungeCurrent.getValueAsDouble();
   }
 
diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
index 22413d0..1d47de9 100644
--- a/src/main/java/frc/robot/subsystems/intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -24,7 +24,7 @@ public Intake(IntakeIO io) {
    *
    * @return runs the pivot at a speed on every iteration until end when it stops the running
    */
-  public Command deploy() {
+  public Command deployOpenLoop() {
     return Commands.runEnd(
         () -> io.setPivotSpeed(IntakeConstants.kPivotMotorSpeed),
         () -> io.setPivotSpeed(0.0),
@@ -36,13 +36,21 @@ public Command deploy() {
    *
    * @return runs the pivot at a speed on every iteration until end when it stops the running
    */
-  public Command retract() {
+  public Command retractOpenLoop() {
     return Commands.runEnd(
         () -> io.setPivotSpeed(-IntakeConstants.kPivotMotorSpeed),
         () -> io.setPivotSpeed(0.0),
         this);
   }
 
+  public Command deployPosition() {
+    return Commands.run(() -> io.setPivotPosition(IntakeConstants.kExtensionPositionRotations), this);
+  }
+
+  public Command retractPosition() {
+    return Commands.run(() -> io.setPivotPosition(0), this);
+  }
+
   /**
    * Command to run the feeder
    *
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
index 9af2f36..fe80a7f 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -1,11 +1,19 @@
 package frc.robot.subsystems.intake;
 
+import com.ctre.phoenix6.configs.Slot0Configs;
+
 public final class IntakeConstants {
   public static final double kPivotMotorSpeed = 0.4;
   public static final double kRollerMotorSpeed = -0.8;
   public static final double kSignificantlyFasterRollerMotorSpeed = -0.75;
 
+  // TODO: Tune
+  public static final double kExtensionPositionRotations = 100.0;
+
   // Change Gear Ratios later
   public static final double kPivotMotorGearRatio = 1.0;
   public static final double kRollerMotorGearRatio = 1.0;
+
+  public static final Slot0Configs kPivotGains =
+      new Slot0Configs().withKP(0.0).withKD(0.0).withKS(0.0).withKV(0.0);
 }
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
index abe8cbb..20eff0c 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
@@ -16,20 +16,23 @@ default void updateInputs(IntakeIOInputs inputs) {}
 
   @AutoLog
   public static class IntakeIOInputs {
-    public double pivotVelocityRadPerSec = 0.0;
-    public double wheelVelocityRadPerSec = 0.0;
-
-    public double pivotPositionRad = 0.0;
-    public double wheelPositionRad = 0.0;
-
-    public double pivotAppliedVolts = 0.0;
-    public double wheelAppliedVolts = 0.0;
-
-    public double pivotCurrentDrawAmps = 0.0;
-    public double wheelCurrentDrawAmps = 0.0;
+    public boolean leftPivotConnected = false;
+    public double leftPivotVelocityRadPerSec = 0.0;
+    public double leftPivotAppliedVolts = 0.0;
+    public double leftPivotCurrentDrawAmps = 0.0;
+
+    public boolean rightPivotConnected = false;
+    public double rightPivotVelocityRadPerSec = 0.0;
+    public double rightPivotAppliedVolts = 0.0;
+    public double rightPivotCurrentDrawAmps = 0.0;
+
+    public boolean driveConnected = false;
+    public double driveVelocityRadPerSec = 0.0;
+    public double driveAppliedVolts = 0.0;
+    public double driveCurrentDrawAmps = 0.0;
   }
 
-  default void setPivotPosition(double positionRad) {}
+  default void setPivotPosition(double positionRotations) {}
 
   /**
    * method to set the speed of the pivot
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java
deleted file mode 100644
index d0da33b..0000000
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java
+++ /dev/null
@@ -1,58 +0,0 @@
-package frc.robot.subsystems.intake;
-
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.revrobotics.PersistMode;
-import com.revrobotics.RelativeEncoder;
-import com.revrobotics.ResetMode;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.config.SparkMaxConfig;
-import edu.wpi.first.math.util.Units;
-import frc.robot.Constants.DeviceIDs;
-
-public class IntakeIOHardware implements IntakeIO {
-  private SparkMax pivotMotor = new SparkMax(DeviceIDs.kIntakePivot, MotorType.kBrushless);
-  private RelativeEncoder pivotEncoder = pivotMotor.getEncoder();
-  private TalonFX driveMotor = new TalonFX(DeviceIDs.kIntakeDrive);
-  private SparkMaxConfig pivotConfig;
-  private TalonFXConfiguration wheelMotorConfig;
-
-  public IntakeIOHardware() {
-    pivotConfig = new SparkMaxConfig();
-    // wheelMotorConfig = new TalonFXConfiguration();
-    // driveMotor.getConfigurator().apply(wheelMotorConfig);
-    pivotMotor.configure(
-        pivotConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
-  }
-
-  @Override
-  public void setPivotPosition(double positionRad) {
-    // TODO Auto-generated method stub
-    IntakeIO.super.setPivotPosition(positionRad);
-  }
-
-  @Override
-  public void setPivotSpeed(double speed) {
-    pivotMotor.set(speed);
-  }
-
-  @Override
-  public void setWheelSpeed(double speed) {
-    driveMotor.set(speed);
-  }
-
-  @Override
-  public void updateInputs(IntakeIOInputs inputs) {
-    inputs.pivotVelocityRadPerSec =
-        Units.rotationsPerMinuteToRadiansPerSecond(pivotEncoder.getVelocity());
-    inputs.wheelVelocityRadPerSec =
-        Units.rotationsToRadians(driveMotor.getVelocity().getValueAsDouble());
-    inputs.pivotPositionRad = Units.rotationsToRadians(pivotEncoder.getPosition());
-    inputs.wheelPositionRad = Units.rotationsToRadians(driveMotor.getPosition().getValueAsDouble());
-    inputs.pivotAppliedVolts = pivotMotor.getAppliedOutput();
-    inputs.wheelAppliedVolts = driveMotor.getTorqueCurrent().getValueAsDouble();
-    inputs.pivotCurrentDrawAmps = pivotMotor.getOutputCurrent();
-    inputs.wheelCurrentDrawAmps = driveMotor.getMotorVoltage().getValueAsDouble();
-  }
-}
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java
index 4d514d0..fccd2a8 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java
@@ -1,64 +1,58 @@
 package frc.robot.subsystems.intake;
 
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.math.system.plant.LinearSystemId;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.simulation.DCMotorSim;
-
 public class IntakeIOSim implements IntakeIO {
 
-  private final DCMotor pivotGearbox = DCMotor.getNEO(1);
-  private final DCMotor wheelGearbox = DCMotor.getKrakenX60(1);
-  private final DCMotorSim pivotSim;
-  private final DCMotorSim wheelSim;
+  // private final DCMotor pivotGearbox = DCMotor.getNEO(1);
+  // private final DCMotor wheelGearbox = DCMotor.getKrakenX60(1);
+  // private final DCMotorSim pivotSim;
+  // private final DCMotorSim wheelSim;
 
-  // private final PIDController pid = new PIDController(1, 0, 0,
-  // Constants.kLoopPeriodSeconds);
+  // // private final PIDController pid = new PIDController(1, 0, 0,
+  // // Constants.kLoopPeriodSeconds);
 
-  private double pivotAppliedVolts = 0.0;
-  private double wheelAppliedVolts = 0.0;
+  // private double pivotAppliedVolts = 0.0;
+  // private double wheelAppliedVolts = 0.0;
 
-  public IntakeIOSim() {
-    pivotSim =
-        new DCMotorSim(
-            LinearSystemId.createDCMotorSystem(
-                pivotGearbox, 0.025, IntakeConstants.kPivotMotorGearRatio),
-            pivotGearbox);
+  // public IntakeIOSim() {
+  //   pivotSim =
+  //       new DCMotorSim(
+  //           LinearSystemId.createDCMotorSystem(
+  //               pivotGearbox, 0.025, IntakeConstants.kPivotMotorGearRatio),
+  //           pivotGearbox);
 
-    wheelSim =
-        new DCMotorSim(
-            LinearSystemId.createDCMotorSystem(
-                wheelGearbox, 0.025, IntakeConstants.kRollerMotorGearRatio),
-            wheelGearbox);
-  }
+  //   wheelSim =
+  //       new DCMotorSim(
+  //           LinearSystemId.createDCMotorSystem(
+  //               wheelGearbox, 0.025, IntakeConstants.kRollerMotorGearRatio),
+  //           wheelGearbox);
+  // }
 
-  @Override
-  public void updateInputs(IntakeIOInputs inputs) {
+  // @Override
+  // public void updateInputs(IntakeIOInputs inputs) {
 
-    pivotAppliedVolts = MathUtil.clamp(pivotAppliedVolts, -12.0, 12.0);
-    wheelAppliedVolts = MathUtil.clamp(wheelAppliedVolts, -12.0, 12.0);
+  //   pivotAppliedVolts = MathUtil.clamp(pivotAppliedVolts, -12.0, 12.0);
+  //   wheelAppliedVolts = MathUtil.clamp(wheelAppliedVolts, -12.0, 12.0);
 
-    pivotSim.setInputVoltage(pivotAppliedVolts);
-    pivotSim.update(0.02);
+  //   pivotSim.setInputVoltage(pivotAppliedVolts);
+  //   pivotSim.update(0.02);
 
-    wheelSim.setInputVoltage(wheelAppliedVolts);
-    wheelSim.update(0.02);
+  //   wheelSim.setInputVoltage(wheelAppliedVolts);
+  //   wheelSim.update(0.02);
 
-    inputs.pivotPositionRad = Units.rotationsToRadians(pivotSim.getAngularPositionRotations());
-    inputs.pivotVelocityRadPerSec = Units.rotationsToRadians(pivotSim.getAngularVelocityRPM());
+  //   inputs.pivotPositionRad = Units.rotationsToRadians(pivotSim.getAngularPositionRotations());
+  //   inputs.pivotVelocityRadPerSec = Units.rotationsToRadians(pivotSim.getAngularVelocityRPM());
 
-    inputs.wheelPositionRad = Units.rotationsToRadians(wheelSim.getAngularPositionRotations());
-    inputs.wheelVelocityRadPerSec = Units.rotationsToRadians(wheelSim.getAngularVelocityRPM());
-  }
+  //   inputs.wheelPositionRad = Units.rotationsToRadians(wheelSim.getAngularPositionRotations());
+  //   inputs.wheelVelocityRadPerSec = Units.rotationsToRadians(wheelSim.getAngularVelocityRPM());
+  // }
 
-  @Override
-  public void setPivotSpeed(double speed) {
-    pivotAppliedVolts = 12 * speed;
-  }
+  // @Override
+  // public void setPivotSpeed(double speed) {
+  //   pivotAppliedVolts = 12 * speed;
+  // }
 
-  @Override
-  public void setWheelSpeed(double speed) {
-    wheelAppliedVolts = 12 * speed;
-  }
+  // @Override
+  // public void setWheelSpeed(double speed) {
+  //   wheelAppliedVolts = 12 * speed;
+  // }
 }
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java
new file mode 100644
index 0000000..83a4ecc
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java
@@ -0,0 +1,118 @@
+package frc.robot.subsystems.intake;
+
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.MotorAlignmentValue;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Voltage;
+import frc.robot.Constants.DeviceIDs;
+
+public class IntakeIOTalonFX implements IntakeIO {
+  private TalonFX leftPivotMotor = new TalonFX(DeviceIDs.kLeftIntakePivot);
+  private TalonFX rightPivotMotor = new TalonFX(DeviceIDs.kLeftIntakePivot);
+  private TalonFX driveMotor = new TalonFX(DeviceIDs.kIntakeDrive);
+
+  private Follower rightPivotFollower =
+      new Follower(DeviceIDs.kLeftIntakePivot, MotorAlignmentValue.Opposed);
+
+  private TalonFXConfiguration leftPivotConfig;
+  private TalonFXConfiguration rightPivotConfig;
+  private TalonFXConfiguration driveMotorConfig;
+
+  private final StatusSignal leftPivotVelocity;
+  private final StatusSignal leftPivotVoltage;
+  private final StatusSignal leftPivotCurrent;
+
+  private final StatusSignal rightPivotVelocity;
+  private final StatusSignal rightPivotVoltage;
+  private final StatusSignal rightPivotCurrent;
+
+  private final StatusSignal driveVelocity;
+  private final StatusSignal driveVoltage;
+  private final StatusSignal driveCurrent;
+
+  private final PositionVoltage positionRequest = new PositionVoltage(0).withSlot(0);
+
+  public IntakeIOTalonFX() {
+    leftPivotConfig = new TalonFXConfiguration().withSlot0(IntakeConstants.kPivotGains);
+    rightPivotConfig = new TalonFXConfiguration().withSlot0(IntakeConstants.kPivotGains);
+
+    leftPivotMotor.setPosition(0);
+    rightPivotMotor.setPosition(0);
+
+    leftPivotMotor.getConfigurator().apply(leftPivotConfig);
+    rightPivotMotor.getConfigurator().apply(rightPivotConfig);
+    driveMotor.getConfigurator().apply(driveMotorConfig);
+
+    rightPivotMotor.setControl(rightPivotFollower);
+
+    leftPivotVelocity = leftPivotMotor.getVelocity();
+    leftPivotVoltage = leftPivotMotor.getMotorVoltage();
+    leftPivotCurrent = leftPivotMotor.getSupplyCurrent();
+
+    rightPivotVelocity = rightPivotMotor.getVelocity();
+    rightPivotVoltage = rightPivotMotor.getMotorVoltage();
+    rightPivotCurrent = rightPivotMotor.getSupplyCurrent();
+
+    driveVelocity = driveMotor.getVelocity();
+    driveVoltage = driveMotor.getMotorVoltage();
+    driveCurrent = driveMotor.getSupplyCurrent();
+
+    BaseStatusSignal.setUpdateFrequencyForAll(
+        50,
+        leftPivotVelocity,
+        leftPivotVoltage,
+        leftPivotCurrent,
+        rightPivotVelocity,
+        rightPivotVoltage,
+        rightPivotCurrent,
+        driveVelocity,
+        driveVoltage,
+        driveCurrent);
+  }
+
+  @Override
+  public void updateInputs(IntakeIOInputs inputs) {
+    inputs.leftPivotConnected =
+        BaseStatusSignal.refreshAll(leftPivotVelocity, leftPivotVoltage, leftPivotCurrent).isOK();
+    inputs.leftPivotVelocityRadPerSec = leftPivotVelocity.getValue().in(RadiansPerSecond);
+    inputs.leftPivotAppliedVolts = leftPivotVoltage.getValueAsDouble();
+    inputs.leftPivotCurrentDrawAmps = leftPivotCurrent.getValueAsDouble();
+
+    inputs.rightPivotConnected =
+        BaseStatusSignal.refreshAll(rightPivotVelocity, rightPivotVoltage, rightPivotCurrent)
+            .isOK();
+    inputs.rightPivotVelocityRadPerSec = rightPivotVelocity.getValue().in(RadiansPerSecond);
+    inputs.rightPivotAppliedVolts = rightPivotVoltage.getValueAsDouble();
+    inputs.rightPivotCurrentDrawAmps = rightPivotCurrent.getValueAsDouble();
+
+    inputs.driveConnected =
+        BaseStatusSignal.refreshAll(driveVelocity, driveVoltage, driveCurrent).isOK();
+    inputs.driveVelocityRadPerSec = driveVelocity.getValue().in(RadiansPerSecond);
+    inputs.driveAppliedVolts = driveVoltage.getValueAsDouble();
+    inputs.driveCurrentDrawAmps = driveCurrent.getValueAsDouble();
+  }
+
+  @Override
+  public void setPivotPosition(double positionRotations) {
+    leftPivotMotor.setControl(positionRequest.withPosition(positionRotations));
+  }
+
+  @Override
+  public void setPivotSpeed(double speed) {
+    leftPivotMotor.set(speed);
+    rightPivotMotor.setControl(rightPivotFollower);
+  }
+
+  @Override
+  public void setWheelSpeed(double speed) {
+    driveMotor.set(speed);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index 9dc80b3..9163893 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -21,9 +21,6 @@ public static final class TurretConstants {
     public static final double kMaxTurretAngleRad = Units.degreesToRadians(120);
     public static final double kAngleTolerance = Units.degreesToRadians(0.5);
 
-    public static final double kLeftMotorId = 12;
-    public static final double kRightMotorId = 13;
-
     // +X = Forward, +Y = Left
     public static final Transform3d kRobotToTurret =
         new Transform3d(Inches.of(3.749), Inches.of(8.186), Inches.of(13.401), Rotation3d.kZero);
@@ -31,10 +28,7 @@ public static final class TurretConstants {
 
   public static final class HoodConstants {
     public static final double kTurretToHoodInches = 1.878;
-    public static final double kGearRatio = 19.2;
-
-    public static final double kLeftHoodID = -1;
-    public static final double kRightHoodID = -1;
+    public static final double kGearRatio = 16 / 1;
 
     public static final double kAngleTolerance = Units.degreesToRadians(5);
 
@@ -51,6 +45,7 @@ public static final class HoodConstants {
                             Inches.of(7.268715), Inches.of(0), Inches.of(0), new Rotation3d())));
 
     public static final double kMinAngleRad = Units.degreesToRadians(0);
+    // TODO: Tune
     public static final double kMaxAngleRad = 5.9;
   }
 
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
index 88467ad..9d2cb8d 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
@@ -6,6 +6,7 @@
 
 import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.PersistMode;
+import com.revrobotics.RelativeEncoder;
 import com.revrobotics.ResetMode;
 import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.FeedbackSensor;
@@ -30,7 +31,10 @@ public class TurretIOSparkMax implements TurretIO {
   private final Debouncer connectedDebouncer = new Debouncer(0.5, DebounceType.kFalling);
 
   public TurretIOSparkMax() {
-    motor = new SparkMax(DeviceIDs.kTurretAzimuth, MotorType.kBrushless);
+    motor =
+        new SparkMax(
+            DeviceIDs.kTurretAzimuth,
+            MotorType.kBrushless);
     encoder = motor.getAbsoluteEncoder();
     motorController = motor.getClosedLoopController();
 
@@ -38,10 +42,17 @@ public TurretIOSparkMax() {
 
     config.idleMode(IdleMode.kCoast);
 
+    config
+        .encoder
+        .positionConversionFactor(
+            2 * Math.PI / TurretConstants.kGearRatio) // No absolute encoder...
+        .velocityConversionFactor(2 * Math.PI / TurretConstants.kGearRatio / 60.0);
+
     config.closedLoop.positionWrappingEnabled(true).feedbackSensor(FeedbackSensor.kPrimaryEncoder);
 
     config.softLimit.reverseSoftLimitEnabled(false).forwardSoftLimitEnabled(false);
 
+    // TODO: Tune
     config.closedLoop.feedForward.kS(0.025 * 12);
     config.closedLoop.p(0.1);
     config.closedLoop.d(0.01);
@@ -54,6 +65,7 @@ public TurretIOSparkMax() {
         () ->
             motor.configure(
                 config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters));
+    tryUntilOk(motor, 5, () -> motor.getEncoder().setPosition(encoder.getPosition()));
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java
index 8cd8e06..b48db78 100644
--- a/src/main/java/frc/robot/subsystems/vision/Vision.java
+++ b/src/main/java/frc/robot/subsystems/vision/Vision.java
@@ -16,7 +16,6 @@
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.subsystems.vision.CameraIO.PoseObservationType;
 import java.util.LinkedList;
@@ -64,8 +63,6 @@ public void periodic() {
       Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]);
     }
 
-    if (Timer.getTimestamp() % 2 < 0.07) System.out.println(inputs[0].connected);
-
     // Initialize logging values
     List allTagPoses = new LinkedList<>();
     List allRobotPoses = new LinkedList<>();
@@ -135,7 +132,6 @@ public void periodic() {
           angularStdDev *= VisionConstants.kCameraStdDevFactors[cameraIndex];
         }
 
-        // if (Timer.getTimestamp() % 2 < 0.07) System.out.println("Linear STDev " + linearStdDev);
         // Send vision observation
         consumer.accept(
             observation.pose().toPose2d(),

From 58fe0abfa061f7bd7212535d8e7b2c98ae51264c Mon Sep 17 00:00:00 2001
From: Maxwell Morgan 
Date: Tue, 17 Mar 2026 17:11:48 -0400
Subject: [PATCH 54/61] Add hood default command

---
 .../frc/robot/control/DefaultControls.java    | 20 +++++++++++-----
 .../frc/robot/subsystems/shooter/Shooter.java | 23 +++++++++++++++----
 .../robot/subsystems/shooter/hood/Hood.java   |  9 +++++---
 .../subsystems/shooter/turret/Turret.java     | 16 -------------
 .../subsystems/shooter/turret/TurretIO.java   |  1 -
 5 files changed, 38 insertions(+), 31 deletions(-)

diff --git a/src/main/java/frc/robot/control/DefaultControls.java b/src/main/java/frc/robot/control/DefaultControls.java
index b42b0ae..98b5ec7 100644
--- a/src/main/java/frc/robot/control/DefaultControls.java
+++ b/src/main/java/frc/robot/control/DefaultControls.java
@@ -2,6 +2,8 @@
 
 import frc.robot.commands.DriveCommands;
 import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.indexer.Indexer;
+import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.shooter.Shooter;
 
 public class DefaultControls implements Configurable {
@@ -9,21 +11,24 @@ public class DefaultControls implements Configurable {
   private final DriverController driver;
   private final DriverController operator;
   private final Drive drive;
-  private final Shooter leftShooter;
-  private final Shooter rightShooter;
+  private final Indexer indexer;
+  private final Intake intake;
+  private final Shooter shooter;
 
   /** Creates a new DefaultControls. */
   public DefaultControls(
       DriverController driver,
       DriverController operator,
       Drive drive,
-      Shooter leftShooter,
-      Shooter rightShooter) {
+      Indexer indexer,
+      Intake intake,
+      Shooter shooter) {
     this.driver = driver;
     this.operator = operator;
     this.drive = drive;
-    this.leftShooter = leftShooter;
-    this.rightShooter = rightShooter;
+    this.indexer = indexer;
+    this.intake = intake;
+    this.shooter = shooter;
   }
 
   /** Configure all default commands for the subsystems (e.g. includes joystick driving). */
@@ -32,5 +37,8 @@ public void configure() {
     drive.setDefaultCommand(
         DriveCommands.joystickDrive(
             drive, () -> -driver.getLeftY(), () -> -driver.getLeftX(), () -> -driver.getRightX()));
+
+    // Avoid the trench
+    shooter.setHoodDefaultCommand(shooter.hoodDown());
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 31c5ec4..a36b17f 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -40,7 +40,8 @@ public void periodic() {
   }
 
   /**
-   * Apply a pre-calculated shooter command to this shooter. This does not require the shooter
+   * Apply a pre-calculated shooter command to this shooter. This does not require
+   * the shooter
    * subsystem - use when combining with other shooters.
    *
    * @param cmd The shot parameters to apply.
@@ -84,12 +85,12 @@ public Command shootAtTargetNoRotation(Supplier targetSupplier) {
         flywheel);
   }
 
-  public Command trackTarget(Supplier targetSupplier) {
-    return turret.trackTarget(targetSupplier);
+  public Command hoodDown() {
+    return hood.down();
   }
 
-  public Command zeroTurret() {
-    return turret.zero();
+  public Command trackTarget(Supplier targetSupplier) {
+    return turret.trackTarget(targetSupplier);
   }
 
   public Command setFlywheelVelocity(double velocityRPM) {
@@ -115,4 +116,16 @@ public void setHoodOpenLoop(double output) {
   public void setTurretOpenLoop(double output) {
     turret.setOpenLoop(output);
   }
+
+  public void setTurretDefaultCommand(Command defaultCommand) {
+    turret.setDefaultCommand(defaultCommand);
+  }
+
+  public void setHoodDefaultCommand(Command defaultCommand) {
+    hood.setDefaultCommand(defaultCommand);
+  }
+
+  public void setFlywheelDefaultCommand(Command defaultCommand) {
+    flywheel.setDefaultCommand(defaultCommand);
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java b/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java
index 0174ecf..49952c1 100644
--- a/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java
+++ b/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java
@@ -55,15 +55,18 @@ public Command trackTarget(Supplier targetSupplier) {
         this);
   }
 
+  public Command down() {
+    return Commands.run(() -> setAngle(0), this);
+  }
+
   /**
    * Sets the hood to the target angle.
    *
    * @param angle The target angle (in radians).
    */
   public void setAngle(double angle) {
-    atGoal =
-        atGoalDebouncer.calculate(
-            Math.abs(angle - inputs.positionRad) < HoodConstants.kAngleTolerance);
+    atGoal = atGoalDebouncer.calculate(
+        Math.abs(angle - inputs.positionRad) < HoodConstants.kAngleTolerance);
     targetAngleRad = angle;
   }
 
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
index d1f1e33..447e29b 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
@@ -32,8 +32,6 @@ public class Turret extends FullSubsystem {
   private boolean atGoal = false;
   private Debouncer atGoalDebouncer = new Debouncer(0.1, DebounceType.kFalling);
 
-  private boolean isZeroed = true;
-
   /** Creates a new Turret. */
   public Turret(TurretIO io) {
     this.io = io;
@@ -44,10 +42,6 @@ public void periodic() {
     io.updateInputs(inputs);
     Logger.processInputs("Turret", inputs);
 
-    if (inputs.limitTriggered) {
-      isZeroed = true;
-    }
-
     RobotVisualizer.getInstance().setTurretAzimuthAngle(Rotation2d.fromRadians(inputs.positionRad));
   }
 
@@ -97,10 +91,6 @@ public Command trackTarget(Supplier targetSupplier) {
         this);
   }
 
-  public Command zero() {
-    return Commands.startEnd(() -> setOpenLoop(0.2), () -> stop()).until(this::isZeroed);
-  }
-
   /**
    * Set the target angle for the turret.
    *
@@ -109,8 +99,6 @@ public Command zero() {
    * @param position A {@link Rotation2d} object representing the target position of the turret.
    */
   public void setPosition(Rotation2d position) {
-    if (!isZeroed) return; // safety
-
     targetAngle = position;
 
     outputs.mode = TurretIOOutputMode.CLOSED_LOOP;
@@ -142,8 +130,4 @@ public double getVelocity() {
   public boolean atGoal() {
     return atGoal;
   }
-
-  public boolean isZeroed() {
-    return isZeroed;
-  }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java
index 4efbe39..b126647 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java
@@ -12,7 +12,6 @@ public static class TurretIOInputs {
     public double velocityRadPerSec = 0.0;
     public double appliedVolts = 0.0;
     public double currentDrawAmps = 0.0;
-    public boolean limitTriggered = false;
   }
 
   public static enum TurretIOOutputMode {

From 300e3070ce83648950a10bd303adc2a9bb190591 Mon Sep 17 00:00:00 2001
From: Legion 
Date: Thu, 19 Mar 2026 22:17:40 -0400
Subject: [PATCH 55/61] Add autos

---
 .../autos/ 1678 Replica Auto RT (2).auto      |  94 +++++++++++++
 .../ DualShot RT Round the World to OP.auto   | 119 ++++++++++++++++
 .../ Half RT Round the World - ASSIST.auto    | 126 +++++++++++++++++
 .../autos/ LT - BPTL - SUTO - OP.auto         |  88 ++++++++++++
 .../autos/1678 Replica Auto LT (1) .auto      |  88 ++++++++++++
 .../autos/1678 Replica Auto LT (2) .auto      |  94 +++++++++++++
 .../autos/1678 Replica Auto RT (1).auto       |  88 ++++++++++++
 src/deploy/pathplanner/autos/8 Auto LT.auto   |  69 +++++++++
 src/deploy/pathplanner/autos/8 Auto RT.auto   |  69 +++++++++
 ...pid stupid auto but on the other side.auto |  69 +++++++++
 .../Brendan's stupid stupid stupid auto.auto  |  69 +++++++++
 .../DualShot LT Round the World to DP.auto    | 113 +++++++++++++++
 .../DualShot LT Round the World to OP.auto    | 113 +++++++++++++++
 .../DualShot MS Round the World to Climb.auto | 112 +++++++++++++++
 .../DualShot MS Round the World to DP.auto    | 119 ++++++++++++++++
 .../DualShot MS Round the World to OP.auto    | 119 ++++++++++++++++
 .../DualShot RT Round the World to Climb.auto | 113 +++++++++++++++
 .../DualShot RT Round the World to DP.auto    | 113 +++++++++++++++
 .../Dualshot LT Round the World to Climb.auto | 106 ++++++++++++++
 .../Half LT Round the World - ASSIST.auto     | 126 +++++++++++++++++
 .../Half LT Round the World - HOARD.auto      | 101 ++++++++++++++
 .../autos/Half RT Round the World - HOAR.auto | 101 ++++++++++++++
 .../autos/LT  - OP - SUTO - Climb.auto        |  81 +++++++++++
 .../autos/LT - BPTL - SUTO - DP.auto          |  88 ++++++++++++
 .../autos/LT - DP - SUTO - Climb.auto         |  81 +++++++++++
 .../autos/LT - DP - SUTO - OP .auto           |  88 ++++++++++++
 .../autos/LT - RT - SUTO Bump Auto.auto       |  69 +++++++++
 .../autos/LT - RT - SUTO Bump Climb Auto.auto |  87 ++++++++++++
 .../pathplanner/autos/LT Locked Auto.auto     |  69 +++++++++
 .../pathplanner/autos/LT Repetitive.auto      | 108 ++++++++++++++
 .../autos/LT Round the World to Climb.auto    | 100 +++++++++++++
 .../autos/LT Round the World to DP.auto       | 107 ++++++++++++++
 .../autos/LT Round the World to OP.auto       | 107 ++++++++++++++
 .../pathplanner/autos/LT to RT MoveShot.auto  | 120 ++++++++++++++++
 .../autos/MS - DP - SUTO - Climb .auto        |  81 +++++++++++
 .../MS - DP - SUTO - OP (No Intake).auto      |  69 +++++++++
 .../autos/MS - LT - RT - SUTO Bump Auto.auto  |  75 ++++++++++
 .../MS - LT - RT - SUTO Bump Climb Auto.auto  |  93 ++++++++++++
 .../pathplanner/autos/MS - LT MoveShot.auto   | 126 +++++++++++++++++
 .../pathplanner/autos/MS - LT Repetitive.auto | 114 +++++++++++++++
 .../autos/MS - OP - SUTO - Climb.auto         |  81 +++++++++++
 .../pathplanner/autos/MS - RT MoveShot.auto   | 126 +++++++++++++++++
 .../pathplanner/autos/MS - RT Repetitive.auto | 114 +++++++++++++++
 .../autos/MS Round the World to Climb.auto    | 106 ++++++++++++++
 .../autos/MS Round the World to DP.auto       | 113 +++++++++++++++
 .../autos/MS Round the World to OP.auto       | 113 +++++++++++++++
 .../autos/MS-LT Round the World - ASSIST.auto | 132 ++++++++++++++++++
 .../autos/MS-LT Round the World - HOARD.auto  | 107 ++++++++++++++
 .../autos/MS-RT Round the World - ASSIST.auto | 132 ++++++++++++++++++
 .../autos/MS-RT Round the World - HOAR.auto   | 107 ++++++++++++++
 .../autos/RT - BPBR - SUTO - DP.auto          |  88 ++++++++++++
 .../autos/RT - BPBR - SUTO - OP.auto          |  88 ++++++++++++
 .../autos/RT - DP - SUTO - Climb.auto         |  81 +++++++++++
 .../autos/RT - DP - SUTO - OP .auto           |  69 +++++++++
 .../autos/RT - LT - SUTO Bump Auto.auto       |  69 +++++++++
 .../autos/RT - LT - SUTO Bump Climb Auto.auto | 106 ++++++++++++++
 .../pathplanner/autos/RT - LT MoveShot.auto   | 120 ++++++++++++++++
 .../autos/RT - OP - SUTO - Climb.auto         |  81 +++++++++++
 .../pathplanner/autos/RT Locked Auto.auto     |  69 +++++++++
 .../pathplanner/autos/RT Repetitive.auto      | 108 ++++++++++++++
 .../autos/RT Round the World to Climb.auto    | 100 +++++++++++++
 .../autos/RT Round the World to DP.auto       | 107 ++++++++++++++
 .../autos/RT Round the World to OP.auto       | 107 ++++++++++++++
 src/deploy/pathplanner/navgrid.json           |   1 +
 .../ 8 Point to ACTUALMiddleBallPit.path      |  54 +++++++
 .../paths/ LT Corner 3 to MidBPTL (2).path    |  59 ++++++++
 .../pathplanner/paths/ MS - RTBump.path       |  54 +++++++
 .../paths/ RT Corner 3 - MidBPBR (2).path     |  59 ++++++++
 .../pathplanner/paths/8Point to MidBPTL.path  |  54 +++++++
 .../paths/ACTUALMiddle to RTBump.path         |  54 +++++++
 .../paths/ACTUALMiddleBallPit to BPTL.path    |  54 +++++++
 .../paths/ACTUALMiddleBallPit to LT.path      |  54 +++++++
 .../paths/ACTUALMiddleBallPit to LTBump.path  |  54 +++++++
 .../paths/ACTUALMiddleBallPit to RT.path      |  54 +++++++
 .../paths/ACTUALMiddleBallPit2 to BPBR.path   |  54 +++++++
 .../pathplanner/paths/BPBR - RTBump.path      |  54 +++++++
 .../paths/BPBR to MiddleBallPit.path          |  54 +++++++
 src/deploy/pathplanner/paths/BPBR to RT.path  |  59 ++++++++
 .../pathplanner/paths/BPBR to RTBump.path     |  54 +++++++
 .../pathplanner/paths/BPBR to RTCorner3.path  |  54 +++++++
 .../paths/BPTL to ACTUALMiddleversion2.path   |  54 +++++++
 src/deploy/pathplanner/paths/BPTL to LT.path  |  59 ++++++++
 .../paths/Bottom to Rotated Top.path          |  54 +++++++
 .../pathplanner/paths/Bottom to Top.path      |  54 +++++++
 .../paths/CornerLine LT to DP.path            |  54 +++++++
 src/deploy/pathplanner/paths/DP to Climb.path |  54 +++++++
 src/deploy/pathplanner/paths/DP to SUTO.path  |  54 +++++++
 .../paths/Fadeaway Top to Bottom.path         |  54 +++++++
 .../pathplanner/paths/LT - MidBPTL.path       |  54 +++++++
 .../paths/LT Corner 3 to 8Point.path          |  54 +++++++
 .../pathplanner/paths/LT Corner 3 to DP.path  |  54 +++++++
 .../LT Corner 3 to MidBPTL (Sped Up).path     |  54 +++++++
 .../paths/LT Corner 3 to MidBPTL.path         |  54 +++++++
 .../paths/LT Corner 3 to SUTO.path            |  54 +++++++
 .../paths/LT CornerLine to SUTO.path          |  54 +++++++
 .../pathplanner/paths/LT To PeakSUTO.path     |  54 +++++++
 src/deploy/pathplanner/paths/LT to BPTL.path  |  59 ++++++++
 src/deploy/pathplanner/paths/LT to Climb.path |  59 ++++++++
 src/deploy/pathplanner/paths/LT to DP.path    |  54 +++++++
 .../pathplanner/paths/LT to LTBump26.path     |  54 +++++++
 src/deploy/pathplanner/paths/LT to OP.path    |  54 +++++++
 .../pathplanner/paths/LT to Rotated BPTL.path |  54 +++++++
 src/deploy/pathplanner/paths/LT to Shoot.path |  59 ++++++++
 .../pathplanner/paths/LTBump - BPTL.path      |  54 +++++++
 .../pathplanner/paths/LTBump to DP.path       |  54 +++++++
 .../paths/LTBump to LTCorner 3.path           |  54 +++++++
 .../pathplanner/paths/LTBump to SUTO.path     |  54 +++++++
 .../pathplanner/paths/MS - LT Corner 3.path   |  54 +++++++
 src/deploy/pathplanner/paths/MS - LTBump.path |  54 +++++++
 .../pathplanner/paths/MS - RT Corner 3.path   |  54 +++++++
 src/deploy/pathplanner/paths/MS to Climb.path |  59 ++++++++
 src/deploy/pathplanner/paths/MS to DP.path    |  54 +++++++
 src/deploy/pathplanner/paths/MS to LT.path    |  54 +++++++
 src/deploy/pathplanner/paths/MS to OP.path    |  54 +++++++
 src/deploy/pathplanner/paths/MS to RT.path    |  54 +++++++
 .../paths/MidBPBR - RT Corner 3.path          |  54 +++++++
 .../paths/MidBPTL - LT Corner 3.path          |  59 ++++++++
 src/deploy/pathplanner/paths/OP to SUTO.path  |  54 +++++++
 .../pathplanner/paths/RT - MidBPBR.path       |  54 +++++++
 .../pathplanner/paths/RT - PeakSUTO.path      |  54 +++++++
 .../RT 8 Point to ACTUALMiddleBallPit2.path   |  54 +++++++
 .../RT Corner 3 - MidBPBR (2) Sped Up.path    |  59 ++++++++
 .../RT Corner 3 - MidBPBR (Sped Up).path      |  59 ++++++++
 .../paths/RT Corner 3 - MidBPBR.path          |  59 ++++++++
 .../paths/RT Corner 3 ro RT 8 Point.path      |  54 +++++++
 .../pathplanner/paths/RT Corner 3 to OP.path  |  54 +++++++
 .../pathplanner/paths/RT Corner3 to SUTO.path |  54 +++++++
 .../pathplanner/paths/RT Corner3-DP.path      |  54 +++++++
 src/deploy/pathplanner/paths/RT to Climb.path |  54 +++++++
 src/deploy/pathplanner/paths/RT to DP.path    |  54 +++++++
 src/deploy/pathplanner/paths/RT to OP.path    |  54 +++++++
 .../pathplanner/paths/RT to RTBump26.path     |  54 +++++++
 .../pathplanner/paths/RT to Rotated BPBR.path |  54 +++++++
 src/deploy/pathplanner/paths/RT to Shoot.path |  59 ++++++++
 .../pathplanner/paths/RTBump - BPBR.path      |  54 +++++++
 .../pathplanner/paths/RTBump to OP.path       |  54 +++++++
 .../paths/RTBump to RTCorner 3.path           |  54 +++++++
 .../pathplanner/paths/RTBump to SUTO.path     |  54 +++++++
 .../paths/Rotated BPBR to RT Corner 3.path    |  63 +++++++++
 .../pathplanner/paths/Rotated BPBR to RT.path |  59 ++++++++
 .../paths/Rotated BPTL - LTBump.path          |  54 +++++++
 .../paths/Rotated BPTL to LT Corrner 3.path   |  59 ++++++++
 .../pathplanner/paths/Rotated BPTL to LT.path |  59 ++++++++
 .../paths/RotatedBPBR to RT Corner 3.path     |  59 ++++++++
 .../paths/RotatedBPTL to LT Corner 3.path     |  54 +++++++
 src/deploy/pathplanner/paths/SUTO - OP.path   |  54 +++++++
 .../pathplanner/paths/SUTO to Climb.path      |  54 +++++++
 src/deploy/pathplanner/paths/SUTO to DP.path  |  59 ++++++++
 src/deploy/pathplanner/paths/SUTO to LT.path  |  54 +++++++
 src/deploy/pathplanner/paths/SUTO to RT.path  |  54 +++++++
 .../pathplanner/paths/Top to Bottom.path      |  54 +++++++
 src/deploy/pathplanner/settings.json          |  54 +++++++
 152 files changed, 11048 insertions(+)
 create mode 100644 src/deploy/pathplanner/autos/ 1678 Replica Auto RT (2).auto
 create mode 100644 src/deploy/pathplanner/autos/ DualShot RT Round the World to OP.auto
 create mode 100644 src/deploy/pathplanner/autos/ Half RT Round the World - ASSIST.auto
 create mode 100644 src/deploy/pathplanner/autos/ LT - BPTL - SUTO - OP.auto
 create mode 100644 src/deploy/pathplanner/autos/1678 Replica Auto LT (1) .auto
 create mode 100644 src/deploy/pathplanner/autos/1678 Replica Auto LT (2) .auto
 create mode 100644 src/deploy/pathplanner/autos/1678 Replica Auto RT (1).auto
 create mode 100644 src/deploy/pathplanner/autos/8 Auto LT.auto
 create mode 100644 src/deploy/pathplanner/autos/8 Auto RT.auto
 create mode 100644 src/deploy/pathplanner/autos/Brendan's stupid stupid stupid auto but on the other side.auto
 create mode 100644 src/deploy/pathplanner/autos/Brendan's stupid stupid stupid auto.auto
 create mode 100644 src/deploy/pathplanner/autos/DualShot LT Round the World to DP.auto
 create mode 100644 src/deploy/pathplanner/autos/DualShot LT Round the World to OP.auto
 create mode 100644 src/deploy/pathplanner/autos/DualShot MS Round the World to Climb.auto
 create mode 100644 src/deploy/pathplanner/autos/DualShot MS Round the World to DP.auto
 create mode 100644 src/deploy/pathplanner/autos/DualShot MS Round the World to OP.auto
 create mode 100644 src/deploy/pathplanner/autos/DualShot RT Round the World to Climb.auto
 create mode 100644 src/deploy/pathplanner/autos/DualShot RT Round the World to DP.auto
 create mode 100644 src/deploy/pathplanner/autos/Dualshot LT Round the World to Climb.auto
 create mode 100644 src/deploy/pathplanner/autos/Half LT Round the World - ASSIST.auto
 create mode 100644 src/deploy/pathplanner/autos/Half LT Round the World - HOARD.auto
 create mode 100644 src/deploy/pathplanner/autos/Half RT Round the World - HOAR.auto
 create mode 100644 src/deploy/pathplanner/autos/LT  - OP - SUTO - Climb.auto
 create mode 100644 src/deploy/pathplanner/autos/LT - BPTL - SUTO - DP.auto
 create mode 100644 src/deploy/pathplanner/autos/LT - DP - SUTO - Climb.auto
 create mode 100644 src/deploy/pathplanner/autos/LT - DP - SUTO - OP .auto
 create mode 100644 src/deploy/pathplanner/autos/LT - RT - SUTO Bump Auto.auto
 create mode 100644 src/deploy/pathplanner/autos/LT - RT - SUTO Bump Climb Auto.auto
 create mode 100644 src/deploy/pathplanner/autos/LT Locked Auto.auto
 create mode 100644 src/deploy/pathplanner/autos/LT Repetitive.auto
 create mode 100644 src/deploy/pathplanner/autos/LT Round the World to Climb.auto
 create mode 100644 src/deploy/pathplanner/autos/LT Round the World to DP.auto
 create mode 100644 src/deploy/pathplanner/autos/LT Round the World to OP.auto
 create mode 100644 src/deploy/pathplanner/autos/LT to RT MoveShot.auto
 create mode 100644 src/deploy/pathplanner/autos/MS - DP - SUTO - Climb .auto
 create mode 100644 src/deploy/pathplanner/autos/MS - DP - SUTO - OP (No Intake).auto
 create mode 100644 src/deploy/pathplanner/autos/MS - LT - RT - SUTO Bump Auto.auto
 create mode 100644 src/deploy/pathplanner/autos/MS - LT - RT - SUTO Bump Climb Auto.auto
 create mode 100644 src/deploy/pathplanner/autos/MS - LT MoveShot.auto
 create mode 100644 src/deploy/pathplanner/autos/MS - LT Repetitive.auto
 create mode 100644 src/deploy/pathplanner/autos/MS - OP - SUTO - Climb.auto
 create mode 100644 src/deploy/pathplanner/autos/MS - RT MoveShot.auto
 create mode 100644 src/deploy/pathplanner/autos/MS - RT Repetitive.auto
 create mode 100644 src/deploy/pathplanner/autos/MS Round the World to Climb.auto
 create mode 100644 src/deploy/pathplanner/autos/MS Round the World to DP.auto
 create mode 100644 src/deploy/pathplanner/autos/MS Round the World to OP.auto
 create mode 100644 src/deploy/pathplanner/autos/MS-LT Round the World - ASSIST.auto
 create mode 100644 src/deploy/pathplanner/autos/MS-LT Round the World - HOARD.auto
 create mode 100644 src/deploy/pathplanner/autos/MS-RT Round the World - ASSIST.auto
 create mode 100644 src/deploy/pathplanner/autos/MS-RT Round the World - HOAR.auto
 create mode 100644 src/deploy/pathplanner/autos/RT - BPBR - SUTO - DP.auto
 create mode 100644 src/deploy/pathplanner/autos/RT - BPBR - SUTO - OP.auto
 create mode 100644 src/deploy/pathplanner/autos/RT - DP - SUTO - Climb.auto
 create mode 100644 src/deploy/pathplanner/autos/RT - DP - SUTO - OP .auto
 create mode 100644 src/deploy/pathplanner/autos/RT - LT - SUTO Bump Auto.auto
 create mode 100644 src/deploy/pathplanner/autos/RT - LT - SUTO Bump Climb Auto.auto
 create mode 100644 src/deploy/pathplanner/autos/RT - LT MoveShot.auto
 create mode 100644 src/deploy/pathplanner/autos/RT - OP - SUTO - Climb.auto
 create mode 100644 src/deploy/pathplanner/autos/RT Locked Auto.auto
 create mode 100644 src/deploy/pathplanner/autos/RT Repetitive.auto
 create mode 100644 src/deploy/pathplanner/autos/RT Round the World to Climb.auto
 create mode 100644 src/deploy/pathplanner/autos/RT Round the World to DP.auto
 create mode 100644 src/deploy/pathplanner/autos/RT Round the World to OP.auto
 create mode 100644 src/deploy/pathplanner/navgrid.json
 create mode 100644 src/deploy/pathplanner/paths/ 8 Point to ACTUALMiddleBallPit.path
 create mode 100644 src/deploy/pathplanner/paths/ LT Corner 3 to MidBPTL (2).path
 create mode 100644 src/deploy/pathplanner/paths/ MS - RTBump.path
 create mode 100644 src/deploy/pathplanner/paths/ RT Corner 3 - MidBPBR (2).path
 create mode 100644 src/deploy/pathplanner/paths/8Point to MidBPTL.path
 create mode 100644 src/deploy/pathplanner/paths/ACTUALMiddle to RTBump.path
 create mode 100644 src/deploy/pathplanner/paths/ACTUALMiddleBallPit to BPTL.path
 create mode 100644 src/deploy/pathplanner/paths/ACTUALMiddleBallPit to LT.path
 create mode 100644 src/deploy/pathplanner/paths/ACTUALMiddleBallPit to LTBump.path
 create mode 100644 src/deploy/pathplanner/paths/ACTUALMiddleBallPit to RT.path
 create mode 100644 src/deploy/pathplanner/paths/ACTUALMiddleBallPit2 to BPBR.path
 create mode 100644 src/deploy/pathplanner/paths/BPBR - RTBump.path
 create mode 100644 src/deploy/pathplanner/paths/BPBR to MiddleBallPit.path
 create mode 100644 src/deploy/pathplanner/paths/BPBR to RT.path
 create mode 100644 src/deploy/pathplanner/paths/BPBR to RTBump.path
 create mode 100644 src/deploy/pathplanner/paths/BPBR to RTCorner3.path
 create mode 100644 src/deploy/pathplanner/paths/BPTL to ACTUALMiddleversion2.path
 create mode 100644 src/deploy/pathplanner/paths/BPTL to LT.path
 create mode 100644 src/deploy/pathplanner/paths/Bottom to Rotated Top.path
 create mode 100644 src/deploy/pathplanner/paths/Bottom to Top.path
 create mode 100644 src/deploy/pathplanner/paths/CornerLine LT to DP.path
 create mode 100644 src/deploy/pathplanner/paths/DP to Climb.path
 create mode 100644 src/deploy/pathplanner/paths/DP to SUTO.path
 create mode 100644 src/deploy/pathplanner/paths/Fadeaway Top to Bottom.path
 create mode 100644 src/deploy/pathplanner/paths/LT - MidBPTL.path
 create mode 100644 src/deploy/pathplanner/paths/LT Corner 3 to 8Point.path
 create mode 100644 src/deploy/pathplanner/paths/LT Corner 3 to DP.path
 create mode 100644 src/deploy/pathplanner/paths/LT Corner 3 to MidBPTL (Sped Up).path
 create mode 100644 src/deploy/pathplanner/paths/LT Corner 3 to MidBPTL.path
 create mode 100644 src/deploy/pathplanner/paths/LT Corner 3 to SUTO.path
 create mode 100644 src/deploy/pathplanner/paths/LT CornerLine to SUTO.path
 create mode 100644 src/deploy/pathplanner/paths/LT To PeakSUTO.path
 create mode 100644 src/deploy/pathplanner/paths/LT to BPTL.path
 create mode 100644 src/deploy/pathplanner/paths/LT to Climb.path
 create mode 100644 src/deploy/pathplanner/paths/LT to DP.path
 create mode 100644 src/deploy/pathplanner/paths/LT to LTBump26.path
 create mode 100644 src/deploy/pathplanner/paths/LT to OP.path
 create mode 100644 src/deploy/pathplanner/paths/LT to Rotated BPTL.path
 create mode 100644 src/deploy/pathplanner/paths/LT to Shoot.path
 create mode 100644 src/deploy/pathplanner/paths/LTBump - BPTL.path
 create mode 100644 src/deploy/pathplanner/paths/LTBump to DP.path
 create mode 100644 src/deploy/pathplanner/paths/LTBump to LTCorner 3.path
 create mode 100644 src/deploy/pathplanner/paths/LTBump to SUTO.path
 create mode 100644 src/deploy/pathplanner/paths/MS - LT Corner 3.path
 create mode 100644 src/deploy/pathplanner/paths/MS - LTBump.path
 create mode 100644 src/deploy/pathplanner/paths/MS - RT Corner 3.path
 create mode 100644 src/deploy/pathplanner/paths/MS to Climb.path
 create mode 100644 src/deploy/pathplanner/paths/MS to DP.path
 create mode 100644 src/deploy/pathplanner/paths/MS to LT.path
 create mode 100644 src/deploy/pathplanner/paths/MS to OP.path
 create mode 100644 src/deploy/pathplanner/paths/MS to RT.path
 create mode 100644 src/deploy/pathplanner/paths/MidBPBR - RT Corner 3.path
 create mode 100644 src/deploy/pathplanner/paths/MidBPTL - LT Corner 3.path
 create mode 100644 src/deploy/pathplanner/paths/OP to SUTO.path
 create mode 100644 src/deploy/pathplanner/paths/RT - MidBPBR.path
 create mode 100644 src/deploy/pathplanner/paths/RT - PeakSUTO.path
 create mode 100644 src/deploy/pathplanner/paths/RT 8 Point to ACTUALMiddleBallPit2.path
 create mode 100644 src/deploy/pathplanner/paths/RT Corner 3 - MidBPBR (2) Sped Up.path
 create mode 100644 src/deploy/pathplanner/paths/RT Corner 3 - MidBPBR (Sped Up).path
 create mode 100644 src/deploy/pathplanner/paths/RT Corner 3 - MidBPBR.path
 create mode 100644 src/deploy/pathplanner/paths/RT Corner 3 ro RT 8 Point.path
 create mode 100644 src/deploy/pathplanner/paths/RT Corner 3 to OP.path
 create mode 100644 src/deploy/pathplanner/paths/RT Corner3 to SUTO.path
 create mode 100644 src/deploy/pathplanner/paths/RT Corner3-DP.path
 create mode 100644 src/deploy/pathplanner/paths/RT to Climb.path
 create mode 100644 src/deploy/pathplanner/paths/RT to DP.path
 create mode 100644 src/deploy/pathplanner/paths/RT to OP.path
 create mode 100644 src/deploy/pathplanner/paths/RT to RTBump26.path
 create mode 100644 src/deploy/pathplanner/paths/RT to Rotated BPBR.path
 create mode 100644 src/deploy/pathplanner/paths/RT to Shoot.path
 create mode 100644 src/deploy/pathplanner/paths/RTBump - BPBR.path
 create mode 100644 src/deploy/pathplanner/paths/RTBump to OP.path
 create mode 100644 src/deploy/pathplanner/paths/RTBump to RTCorner 3.path
 create mode 100644 src/deploy/pathplanner/paths/RTBump to SUTO.path
 create mode 100644 src/deploy/pathplanner/paths/Rotated BPBR to RT Corner 3.path
 create mode 100644 src/deploy/pathplanner/paths/Rotated BPBR to RT.path
 create mode 100644 src/deploy/pathplanner/paths/Rotated BPTL - LTBump.path
 create mode 100644 src/deploy/pathplanner/paths/Rotated BPTL to LT Corrner 3.path
 create mode 100644 src/deploy/pathplanner/paths/Rotated BPTL to LT.path
 create mode 100644 src/deploy/pathplanner/paths/RotatedBPBR to RT Corner 3.path
 create mode 100644 src/deploy/pathplanner/paths/RotatedBPTL to LT Corner 3.path
 create mode 100644 src/deploy/pathplanner/paths/SUTO - OP.path
 create mode 100644 src/deploy/pathplanner/paths/SUTO to Climb.path
 create mode 100644 src/deploy/pathplanner/paths/SUTO to DP.path
 create mode 100644 src/deploy/pathplanner/paths/SUTO to LT.path
 create mode 100644 src/deploy/pathplanner/paths/SUTO to RT.path
 create mode 100644 src/deploy/pathplanner/paths/Top to Bottom.path
 create mode 100644 src/deploy/pathplanner/settings.json

diff --git a/src/deploy/pathplanner/autos/ 1678 Replica Auto RT (2).auto b/src/deploy/pathplanner/autos/ 1678 Replica Auto RT (2).auto
new file mode 100644
index 0000000..0469d13
--- /dev/null
+++ b/src/deploy/pathplanner/autos/ 1678 Replica Auto RT (2).auto	
@@ -0,0 +1,94 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT to Rotated BPBR"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BPBR to MiddleBallPit"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "ACTUALMiddleBallPit to RT"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT to RTBump26"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RTBump to OP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "ChudBot Autos",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/ DualShot RT Round the World to OP.auto b/src/deploy/pathplanner/autos/ DualShot RT Round the World to OP.auto
new file mode 100644
index 0000000..6315f9c
--- /dev/null
+++ b/src/deploy/pathplanner/autos/ DualShot RT Round the World to OP.auto	
@@ -0,0 +1,119 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT to Rotated BPBR"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Bottom to Top"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPTL to LT"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT to Shoot"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO - OP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "ATW DualShot",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/ Half RT Round the World - ASSIST.auto b/src/deploy/pathplanner/autos/ Half RT Round the World - ASSIST.auto
new file mode 100644
index 0000000..4b94084
--- /dev/null
+++ b/src/deploy/pathplanner/autos/ Half RT Round the World - ASSIST.auto	
@@ -0,0 +1,126 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT to Rotated BPBR"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Bottom to Top"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Rotated BPTL to LT"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT to BPTL"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPBR to RT"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "PeoplePleaser",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/ LT - BPTL - SUTO - OP.auto b/src/deploy/pathplanner/autos/ LT - BPTL - SUTO - OP.auto
new file mode 100644
index 0000000..43bca40
--- /dev/null
+++ b/src/deploy/pathplanner/autos/ LT - BPTL - SUTO - OP.auto	
@@ -0,0 +1,88 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT - MidBPTL"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.9
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "intake"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "MidBPTL - LT Corner 3"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT Corner 3 to SUTO"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO - OP"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "ChudBot Autos",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/1678 Replica Auto LT (1) .auto b/src/deploy/pathplanner/autos/1678 Replica Auto LT (1) .auto
new file mode 100644
index 0000000..a291643
--- /dev/null
+++ b/src/deploy/pathplanner/autos/1678 Replica Auto LT (1) .auto	
@@ -0,0 +1,88 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT to Rotated BPTL"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BPTL to ACTUALMiddleversion2"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "ACTUALMiddleBallPit to LTBump"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LTBump to DP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "ChudBot Autos",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/1678 Replica Auto LT (2) .auto b/src/deploy/pathplanner/autos/1678 Replica Auto LT (2) .auto
new file mode 100644
index 0000000..85f38f7
--- /dev/null
+++ b/src/deploy/pathplanner/autos/1678 Replica Auto LT (2) .auto	
@@ -0,0 +1,94 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT to Rotated BPTL"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BPTL to ACTUALMiddleversion2"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "ACTUALMiddleBallPit to LT"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT to LTBump26"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LTBump to DP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "ChudBot Autos",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/1678 Replica Auto RT (1).auto b/src/deploy/pathplanner/autos/1678 Replica Auto RT (1).auto
new file mode 100644
index 0000000..8917117
--- /dev/null
+++ b/src/deploy/pathplanner/autos/1678 Replica Auto RT (1).auto	
@@ -0,0 +1,88 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT to Rotated BPBR"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BPBR to MiddleBallPit"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "ACTUALMiddle to RTBump"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RTBump to OP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "ChudBot Autos",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/8 Auto LT.auto b/src/deploy/pathplanner/autos/8 Auto LT.auto
new file mode 100644
index 0000000..94d837f
--- /dev/null
+++ b/src/deploy/pathplanner/autos/8 Auto LT.auto	
@@ -0,0 +1,69 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LTBump to LTCorner 3"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT Corner 3 to 8Point"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": " 8 Point to ACTUALMiddleBallPit"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "ACTUALMiddleBallPit to BPTL"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPTL to LT Corrner 3"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "ChudBot Autos",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/8 Auto RT.auto b/src/deploy/pathplanner/autos/8 Auto RT.auto
new file mode 100644
index 0000000..49fc4a6
--- /dev/null
+++ b/src/deploy/pathplanner/autos/8 Auto RT.auto	
@@ -0,0 +1,69 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RTBump to RTCorner 3"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT Corner 3 ro RT 8 Point"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT 8 Point to ACTUALMiddleBallPit2"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "ACTUALMiddleBallPit2 to BPBR"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BPBR to RTCorner3"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "ChudBot Autos",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/Brendan's stupid stupid stupid auto but on the other side.auto b/src/deploy/pathplanner/autos/Brendan's stupid stupid stupid auto but on the other side.auto
new file mode 100644
index 0000000..d1a4665
--- /dev/null
+++ b/src/deploy/pathplanner/autos/Brendan's stupid stupid stupid auto but on the other side.auto	
@@ -0,0 +1,69 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT to Rotated BPTL"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BPTL to ACTUALMiddleversion2"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "ACTUALMiddleBallPit to LTBump"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LTBump to DP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Brendan Wants It",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/Brendan's stupid stupid stupid auto.auto b/src/deploy/pathplanner/autos/Brendan's stupid stupid stupid auto.auto
new file mode 100644
index 0000000..387b158
--- /dev/null
+++ b/src/deploy/pathplanner/autos/Brendan's stupid stupid stupid auto.auto	
@@ -0,0 +1,69 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT to Rotated BPBR"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "BPBR to MiddleBallPit"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "ACTUALMiddle to RTBump"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RTBump to OP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Brendan Wants It",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/DualShot LT Round the World to DP.auto b/src/deploy/pathplanner/autos/DualShot LT Round the World to DP.auto
new file mode 100644
index 0000000..9053571
--- /dev/null
+++ b/src/deploy/pathplanner/autos/DualShot LT Round the World to DP.auto	
@@ -0,0 +1,113 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT to BPTL"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPBR to RT"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT to Shoot"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to DP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "ATW DualShot",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/DualShot LT Round the World to OP.auto b/src/deploy/pathplanner/autos/DualShot LT Round the World to OP.auto
new file mode 100644
index 0000000..839ddc5
--- /dev/null
+++ b/src/deploy/pathplanner/autos/DualShot LT Round the World to OP.auto	
@@ -0,0 +1,113 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT to BPTL"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPBR to RT"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT to Shoot"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO - OP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "ATW DualShot",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/DualShot MS Round the World to Climb.auto b/src/deploy/pathplanner/autos/DualShot MS Round the World to Climb.auto
new file mode 100644
index 0000000..fd7e7ee
--- /dev/null
+++ b/src/deploy/pathplanner/autos/DualShot MS Round the World to Climb.auto	
@@ -0,0 +1,112 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "MS to LT"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT to BPTL"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPBR to RT"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT to Shoot"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to Climb"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.75
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "climb"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "ATW DualShot",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/DualShot MS Round the World to DP.auto b/src/deploy/pathplanner/autos/DualShot MS Round the World to DP.auto
new file mode 100644
index 0000000..b67de30
--- /dev/null
+++ b/src/deploy/pathplanner/autos/DualShot MS Round the World to DP.auto	
@@ -0,0 +1,119 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "MS to LT"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT to BPTL"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPBR to RT"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT to Shoot"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to DP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "ATW DualShot",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/DualShot MS Round the World to OP.auto b/src/deploy/pathplanner/autos/DualShot MS Round the World to OP.auto
new file mode 100644
index 0000000..226ea9e
--- /dev/null
+++ b/src/deploy/pathplanner/autos/DualShot MS Round the World to OP.auto	
@@ -0,0 +1,119 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "MS to LT"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT to BPTL"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPBR to RT"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT to Shoot"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO - OP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "ATW DualShot",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/DualShot RT Round the World to Climb.auto b/src/deploy/pathplanner/autos/DualShot RT Round the World to Climb.auto
new file mode 100644
index 0000000..1685df2
--- /dev/null
+++ b/src/deploy/pathplanner/autos/DualShot RT Round the World to Climb.auto	
@@ -0,0 +1,113 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT to Rotated BPBR"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Bottom to Top"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPTL to LT"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT to Shoot"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to Climb"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "climb"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "ATW DualShot",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/DualShot RT Round the World to DP.auto b/src/deploy/pathplanner/autos/DualShot RT Round the World to DP.auto
new file mode 100644
index 0000000..e86fab2
--- /dev/null
+++ b/src/deploy/pathplanner/autos/DualShot RT Round the World to DP.auto	
@@ -0,0 +1,113 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT to Rotated BPBR"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Bottom to Top"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPTL to LT"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT to Shoot"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to DP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "ATW DualShot",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/Dualshot LT Round the World to Climb.auto b/src/deploy/pathplanner/autos/Dualshot LT Round the World to Climb.auto
new file mode 100644
index 0000000..b18d8ca
--- /dev/null
+++ b/src/deploy/pathplanner/autos/Dualshot LT Round the World to Climb.auto	
@@ -0,0 +1,106 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT to BPTL"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPBR to RT"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT to Shoot"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to Climb"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.75
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "climb"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "ATW DualShot",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/Half LT Round the World - ASSIST.auto b/src/deploy/pathplanner/autos/Half LT Round the World - ASSIST.auto
new file mode 100644
index 0000000..6f22218
--- /dev/null
+++ b/src/deploy/pathplanner/autos/Half LT Round the World - ASSIST.auto	
@@ -0,0 +1,126 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT to BPTL"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Rotated BPBR to RT"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT to Rotated BPBR"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Bottom to Top"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPTL to LT"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "PeoplePleaser",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/Half LT Round the World - HOARD.auto b/src/deploy/pathplanner/autos/Half LT Round the World - HOARD.auto
new file mode 100644
index 0000000..0b68281
--- /dev/null
+++ b/src/deploy/pathplanner/autos/Half LT Round the World - HOARD.auto	
@@ -0,0 +1,101 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT to Rotated BPTL"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPBR to RT"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT to Rotated BPBR"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Bottom to Top"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPTL to LT"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "PeoplePleaser",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/Half RT Round the World - HOAR.auto b/src/deploy/pathplanner/autos/Half RT Round the World - HOAR.auto
new file mode 100644
index 0000000..817b938
--- /dev/null
+++ b/src/deploy/pathplanner/autos/Half RT Round the World - HOAR.auto	
@@ -0,0 +1,101 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT to Rotated BPBR"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Bottom to Top"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPTL to LT"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT to BPTL"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BPBR to RT"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "PeoplePleaser",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/LT  - OP - SUTO - Climb.auto b/src/deploy/pathplanner/autos/LT  - OP - SUTO - Climb.auto
new file mode 100644
index 0000000..461b5ff
--- /dev/null
+++ b/src/deploy/pathplanner/autos/LT  - OP - SUTO - Climb.auto	
@@ -0,0 +1,81 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT to OP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "OP to SUTO"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to Climb"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.75
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "climb"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "LT Auditorium Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/LT - BPTL - SUTO - DP.auto b/src/deploy/pathplanner/autos/LT - BPTL - SUTO - DP.auto
new file mode 100644
index 0000000..00ac754
--- /dev/null
+++ b/src/deploy/pathplanner/autos/LT - BPTL - SUTO - DP.auto	
@@ -0,0 +1,88 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT - MidBPTL"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.9
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "intake"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "MidBPTL - LT Corner 3"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT Corner 3 to SUTO"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to DP"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "ChudBot Autos",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/LT - DP - SUTO - Climb.auto b/src/deploy/pathplanner/autos/LT - DP - SUTO - Climb.auto
new file mode 100644
index 0000000..412c626
--- /dev/null
+++ b/src/deploy/pathplanner/autos/LT - DP - SUTO - Climb.auto	
@@ -0,0 +1,81 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT to DP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "DP to SUTO"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to Climb"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.75
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "climb"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "LT Auditorium Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/LT - DP - SUTO - OP .auto b/src/deploy/pathplanner/autos/LT - DP - SUTO - OP .auto
new file mode 100644
index 0000000..9bb5505
--- /dev/null
+++ b/src/deploy/pathplanner/autos/LT - DP - SUTO - OP .auto	
@@ -0,0 +1,88 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT to DP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "DP to SUTO"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO - OP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "LT Auditorium Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/LT - RT - SUTO Bump Auto.auto b/src/deploy/pathplanner/autos/LT - RT - SUTO Bump Auto.auto
new file mode 100644
index 0000000..fe41a8f
--- /dev/null
+++ b/src/deploy/pathplanner/autos/LT - RT - SUTO Bump Auto.auto	
@@ -0,0 +1,69 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LTBump - BPTL"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BPBR to RTBump"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RTBump to SUTO"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Bump Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/LT - RT - SUTO Bump Climb Auto.auto b/src/deploy/pathplanner/autos/LT - RT - SUTO Bump Climb Auto.auto
new file mode 100644
index 0000000..22d955b
--- /dev/null
+++ b/src/deploy/pathplanner/autos/LT - RT - SUTO Bump Climb Auto.auto	
@@ -0,0 +1,87 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LTBump - BPTL"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BPBR to RTBump"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RTBump to SUTO"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to Climb"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.75
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "climb"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Bump Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/LT Locked Auto.auto b/src/deploy/pathplanner/autos/LT Locked Auto.auto
new file mode 100644
index 0000000..4e69d14
--- /dev/null
+++ b/src/deploy/pathplanner/autos/LT Locked Auto.auto	
@@ -0,0 +1,69 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT to BPTL"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPBR to RT"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT - PeakSUTO"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Locked Autos",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/LT Repetitive.auto b/src/deploy/pathplanner/autos/LT Repetitive.auto
new file mode 100644
index 0000000..a4456cc
--- /dev/null
+++ b/src/deploy/pathplanner/autos/LT Repetitive.auto	
@@ -0,0 +1,108 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT - MidBPTL"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.45
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "intake"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "MidBPTL - LT Corner 3"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": " LT Corner 3 to MidBPTL (2)"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.44
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "intake"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Repetitive Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/LT Round the World to Climb.auto b/src/deploy/pathplanner/autos/LT Round the World to Climb.auto
new file mode 100644
index 0000000..ef0b581
--- /dev/null
+++ b/src/deploy/pathplanner/autos/LT Round the World to Climb.auto	
@@ -0,0 +1,100 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT to BPTL"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPBR to RT"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT to Shoot"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to Climb"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.75
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "climb"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "LT Stem Lab Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/LT Round the World to DP.auto b/src/deploy/pathplanner/autos/LT Round the World to DP.auto
new file mode 100644
index 0000000..2640d9d
--- /dev/null
+++ b/src/deploy/pathplanner/autos/LT Round the World to DP.auto	
@@ -0,0 +1,107 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT to BPTL"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPBR to RT"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT to Shoot"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to DP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "LT Stem Lab Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/LT Round the World to OP.auto b/src/deploy/pathplanner/autos/LT Round the World to OP.auto
new file mode 100644
index 0000000..f7259ab
--- /dev/null
+++ b/src/deploy/pathplanner/autos/LT Round the World to OP.auto	
@@ -0,0 +1,107 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT to BPTL"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPBR to RT"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT to Shoot"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO - OP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "LT Stem Lab Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/LT to RT MoveShot.auto b/src/deploy/pathplanner/autos/LT to RT MoveShot.auto
new file mode 100644
index 0000000..fd2a22f
--- /dev/null
+++ b/src/deploy/pathplanner/autos/LT to RT MoveShot.auto	
@@ -0,0 +1,120 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT to BPTL"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RotatedBPBR to RT Corner 3"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT Corner3-DP"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "DP to Climb"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 0.75
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "climb"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Useless Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/MS - DP - SUTO - Climb .auto b/src/deploy/pathplanner/autos/MS - DP - SUTO - Climb .auto
new file mode 100644
index 0000000..456fa42
--- /dev/null
+++ b/src/deploy/pathplanner/autos/MS - DP - SUTO - Climb .auto	
@@ -0,0 +1,81 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "MS to DP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "DP to SUTO"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to Climb"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.75
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "climb"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "MS Auditorium Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/MS - DP - SUTO - OP (No Intake).auto b/src/deploy/pathplanner/autos/MS - DP - SUTO - OP (No Intake).auto
new file mode 100644
index 0000000..935cf3d
--- /dev/null
+++ b/src/deploy/pathplanner/autos/MS - DP - SUTO - OP (No Intake).auto	
@@ -0,0 +1,69 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "MS to DP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "DP to SUTO"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO - OP"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "MS Auditorium Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/MS - LT - RT - SUTO Bump Auto.auto b/src/deploy/pathplanner/autos/MS - LT - RT - SUTO Bump Auto.auto
new file mode 100644
index 0000000..b5cb7e6
--- /dev/null
+++ b/src/deploy/pathplanner/autos/MS - LT - RT - SUTO Bump Auto.auto	
@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "MS - LTBump"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LTBump - BPTL"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BPBR to RTBump"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RTBump to SUTO"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Bump Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/MS - LT - RT - SUTO Bump Climb Auto.auto b/src/deploy/pathplanner/autos/MS - LT - RT - SUTO Bump Climb Auto.auto
new file mode 100644
index 0000000..70fec3b
--- /dev/null
+++ b/src/deploy/pathplanner/autos/MS - LT - RT - SUTO Bump Climb Auto.auto	
@@ -0,0 +1,93 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "MS - LTBump"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LTBump - BPTL"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BPBR to RTBump"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RTBump to SUTO"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to Climb"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.75
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "climb"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Bump Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/MS - LT MoveShot.auto b/src/deploy/pathplanner/autos/MS - LT MoveShot.auto
new file mode 100644
index 0000000..cf8e487
--- /dev/null
+++ b/src/deploy/pathplanner/autos/MS - LT MoveShot.auto	
@@ -0,0 +1,126 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "MS to LT"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT to BPTL"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RotatedBPBR to RT Corner 3"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT Corner3-DP"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "DP to Climb"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 0.75
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "climb"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Useless Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/MS - LT Repetitive.auto b/src/deploy/pathplanner/autos/MS - LT Repetitive.auto
new file mode 100644
index 0000000..4b64824
--- /dev/null
+++ b/src/deploy/pathplanner/autos/MS - LT Repetitive.auto	
@@ -0,0 +1,114 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "MS - LT Corner 3"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT Corner 3 to MidBPTL (Sped Up)"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.25
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "intake"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "MidBPTL - LT Corner 3"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": " LT Corner 3 to MidBPTL (2)"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.44
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "intake"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Repetitive Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/MS - OP - SUTO - Climb.auto b/src/deploy/pathplanner/autos/MS - OP - SUTO - Climb.auto
new file mode 100644
index 0000000..fc8b805
--- /dev/null
+++ b/src/deploy/pathplanner/autos/MS - OP - SUTO - Climb.auto	
@@ -0,0 +1,81 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "MS to OP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "OP to SUTO"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to Climb"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.75
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "climb"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "MS Auditorium Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/MS - RT MoveShot.auto b/src/deploy/pathplanner/autos/MS - RT MoveShot.auto
new file mode 100644
index 0000000..37c9406
--- /dev/null
+++ b/src/deploy/pathplanner/autos/MS - RT MoveShot.auto	
@@ -0,0 +1,126 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "MS to RT"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT to Rotated BPBR"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Bottom to Top"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RotatedBPTL to LT Corner 3"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT Corner 3 to DP"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "DP to Climb"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 0.75
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "climb"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Useless Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/MS - RT Repetitive.auto b/src/deploy/pathplanner/autos/MS - RT Repetitive.auto
new file mode 100644
index 0000000..841fb5c
--- /dev/null
+++ b/src/deploy/pathplanner/autos/MS - RT Repetitive.auto	
@@ -0,0 +1,114 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "MS - RT Corner 3"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT Corner 3 - MidBPBR (Sped Up)"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.12
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "intake"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "MidBPBR - RT Corner 3"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT Corner 3 - MidBPBR (2) Sped Up"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.6
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "intake"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Repetitive Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/MS Round the World to Climb.auto b/src/deploy/pathplanner/autos/MS Round the World to Climb.auto
new file mode 100644
index 0000000..d7772a2
--- /dev/null
+++ b/src/deploy/pathplanner/autos/MS Round the World to Climb.auto	
@@ -0,0 +1,106 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "MS to LT"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT to BPTL"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPBR to RT"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT to Shoot"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to Climb"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.75
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "climb"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "MS Stem Lab Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/MS Round the World to DP.auto b/src/deploy/pathplanner/autos/MS Round the World to DP.auto
new file mode 100644
index 0000000..ea83f5c
--- /dev/null
+++ b/src/deploy/pathplanner/autos/MS Round the World to DP.auto	
@@ -0,0 +1,113 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "MS to LT"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT to BPTL"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPBR to RT"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT to Shoot"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to DP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "MS Stem Lab Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/MS Round the World to OP.auto b/src/deploy/pathplanner/autos/MS Round the World to OP.auto
new file mode 100644
index 0000000..2b5c88d
--- /dev/null
+++ b/src/deploy/pathplanner/autos/MS Round the World to OP.auto	
@@ -0,0 +1,113 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "MS to LT"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT to BPTL"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPBR to RT"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT to Shoot"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO - OP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "MS Stem Lab Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/MS-LT Round the World - ASSIST.auto b/src/deploy/pathplanner/autos/MS-LT Round the World - ASSIST.auto
new file mode 100644
index 0000000..741687d
--- /dev/null
+++ b/src/deploy/pathplanner/autos/MS-LT Round the World - ASSIST.auto	
@@ -0,0 +1,132 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "MS to LT"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT to BPTL"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Rotated BPBR to RT"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT to Rotated BPBR"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Bottom to Top"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPTL to LT"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "PeoplePleaser",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/MS-LT Round the World - HOARD.auto b/src/deploy/pathplanner/autos/MS-LT Round the World - HOARD.auto
new file mode 100644
index 0000000..9c66bbe
--- /dev/null
+++ b/src/deploy/pathplanner/autos/MS-LT Round the World - HOARD.auto	
@@ -0,0 +1,107 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "MS to LT"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT to Rotated BPTL"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPBR to RT"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT to Rotated BPBR"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Bottom to Top"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPTL to LT"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "PeoplePleaser",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/MS-RT Round the World - ASSIST.auto b/src/deploy/pathplanner/autos/MS-RT Round the World - ASSIST.auto
new file mode 100644
index 0000000..f9b3c12
--- /dev/null
+++ b/src/deploy/pathplanner/autos/MS-RT Round the World - ASSIST.auto	
@@ -0,0 +1,132 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "MS to RT"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT to Rotated BPBR"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Bottom to Top"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Rotated BPTL to LT"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT to BPTL"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPBR to RT"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "PeoplePleaser",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/MS-RT Round the World - HOAR.auto b/src/deploy/pathplanner/autos/MS-RT Round the World - HOAR.auto
new file mode 100644
index 0000000..532f62b
--- /dev/null
+++ b/src/deploy/pathplanner/autos/MS-RT Round the World - HOAR.auto	
@@ -0,0 +1,107 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "MS to RT"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT to Rotated BPBR"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Bottom to Top"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPTL to LT"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT to BPTL"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Top to Bottom"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BPBR to RT"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "PeoplePleaser",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/RT - BPBR - SUTO - DP.auto b/src/deploy/pathplanner/autos/RT - BPBR - SUTO - DP.auto
new file mode 100644
index 0000000..628e48f
--- /dev/null
+++ b/src/deploy/pathplanner/autos/RT - BPBR - SUTO - DP.auto	
@@ -0,0 +1,88 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT - MidBPBR"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.9
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "intake"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "MidBPBR - RT Corner 3"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT Corner3 to SUTO"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to DP"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "ChudBot Autos",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/RT - BPBR - SUTO - OP.auto b/src/deploy/pathplanner/autos/RT - BPBR - SUTO - OP.auto
new file mode 100644
index 0000000..f5a69b9
--- /dev/null
+++ b/src/deploy/pathplanner/autos/RT - BPBR - SUTO - OP.auto	
@@ -0,0 +1,88 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT - MidBPBR"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.9
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "intake"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "MidBPBR - RT Corner 3"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT Corner3 to SUTO"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO - OP"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "ChudBot Autos",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/RT - DP - SUTO - Climb.auto b/src/deploy/pathplanner/autos/RT - DP - SUTO - Climb.auto
new file mode 100644
index 0000000..eca1895
--- /dev/null
+++ b/src/deploy/pathplanner/autos/RT - DP - SUTO - Climb.auto	
@@ -0,0 +1,81 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT to DP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "DP to SUTO"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to Climb"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.75
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "climb"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "RT Auditorium Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/RT - DP - SUTO - OP .auto b/src/deploy/pathplanner/autos/RT - DP - SUTO - OP .auto
new file mode 100644
index 0000000..3ab0338
--- /dev/null
+++ b/src/deploy/pathplanner/autos/RT - DP - SUTO - OP .auto	
@@ -0,0 +1,69 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT to DP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "DP to SUTO"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO - OP"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "RT Auditorium Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/RT - LT - SUTO Bump Auto.auto b/src/deploy/pathplanner/autos/RT - LT - SUTO Bump Auto.auto
new file mode 100644
index 0000000..2b51fb7
--- /dev/null
+++ b/src/deploy/pathplanner/autos/RT - LT - SUTO Bump Auto.auto	
@@ -0,0 +1,69 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RTBump - BPBR"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Bottom to Top"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPTL - LTBump"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LTBump to SUTO"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Bump Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/RT - LT - SUTO Bump Climb Auto.auto b/src/deploy/pathplanner/autos/RT - LT - SUTO Bump Climb Auto.auto
new file mode 100644
index 0000000..acd1ae1
--- /dev/null
+++ b/src/deploy/pathplanner/autos/RT - LT - SUTO Bump Climb Auto.auto	
@@ -0,0 +1,106 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RTBump - BPBR"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Bottom to Top"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPTL - LTBump"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LTBump to SUTO"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to Climb"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.75
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "climb"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Bump Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/RT - LT MoveShot.auto b/src/deploy/pathplanner/autos/RT - LT MoveShot.auto
new file mode 100644
index 0000000..c720bd9
--- /dev/null
+++ b/src/deploy/pathplanner/autos/RT - LT MoveShot.auto	
@@ -0,0 +1,120 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT to Rotated BPBR"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Bottom to Top"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPTL to LT Corrner 3"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "LT Corner 3 to DP"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "DP to Climb"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 0.75
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "climb"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Useless Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/RT - OP - SUTO - Climb.auto b/src/deploy/pathplanner/autos/RT - OP - SUTO - Climb.auto
new file mode 100644
index 0000000..dad69ed
--- /dev/null
+++ b/src/deploy/pathplanner/autos/RT - OP - SUTO - Climb.auto	
@@ -0,0 +1,81 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT to OP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "OP to SUTO"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to Climb"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.75
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "climb"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "RT Auditorium Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/RT Locked Auto.auto b/src/deploy/pathplanner/autos/RT Locked Auto.auto
new file mode 100644
index 0000000..7512a6c
--- /dev/null
+++ b/src/deploy/pathplanner/autos/RT Locked Auto.auto	
@@ -0,0 +1,69 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RT to Rotated BPBR"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Bottom to Top"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPTL to LT"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT To PeakSUTO"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Locked Autos",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/RT Repetitive.auto b/src/deploy/pathplanner/autos/RT Repetitive.auto
new file mode 100644
index 0000000..8c7c550
--- /dev/null
+++ b/src/deploy/pathplanner/autos/RT Repetitive.auto	
@@ -0,0 +1,108 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT - MidBPBR"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.7
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "intake"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "MidBPBR - RT Corner 3"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": " RT Corner 3 - MidBPBR (2)"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.9
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "intake"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Repetitive Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/RT Round the World to Climb.auto b/src/deploy/pathplanner/autos/RT Round the World to Climb.auto
new file mode 100644
index 0000000..a8f8555
--- /dev/null
+++ b/src/deploy/pathplanner/autos/RT Round the World to Climb.auto	
@@ -0,0 +1,100 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT to Rotated BPBR"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Bottom to Top"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPTL to LT"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT to Shoot"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to Climb"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.75
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "climb"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "RT Stem Lab Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/RT Round the World to DP.auto b/src/deploy/pathplanner/autos/RT Round the World to DP.auto
new file mode 100644
index 0000000..d47fc6c
--- /dev/null
+++ b/src/deploy/pathplanner/autos/RT Round the World to DP.auto	
@@ -0,0 +1,107 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT to Rotated BPBR"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Bottom to Top"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPTL to LT"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT to Shoot"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO to DP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "RT Stem Lab Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/RT Round the World to OP.auto b/src/deploy/pathplanner/autos/RT Round the World to OP.auto
new file mode 100644
index 0000000..6ae6d41
--- /dev/null
+++ b/src/deploy/pathplanner/autos/RT Round the World to OP.auto	
@@ -0,0 +1,107 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RT to Rotated BPBR"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Bottom to Top"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Rotated BPTL to LT"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LT to Shoot"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.5
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SUTO - OP"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "RT Stem Lab Auto",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/navgrid.json b/src/deploy/pathplanner/navgrid.json
new file mode 100644
index 0000000..6d5fbd8
--- /dev/null
+++ b/src/deploy/pathplanner/navgrid.json
@@ -0,0 +1 @@
+{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,true,true,false,true,true,true,true,true,true,true,true,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,true,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,true,true,true,false,false,false,false,false,true,true,true,true,true,true,false,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,true,true,false,false,false,false,false,true,true,true,true,true,true,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,true,true,false,false,false,false,false,true,true,true,true,true,true,false,true,false,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,false,false,false,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,false,true,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,true,false,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,false,false,false,false,true,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/ 8 Point to ACTUALMiddleBallPit.path b/src/deploy/pathplanner/paths/ 8 Point to ACTUALMiddleBallPit.path
new file mode 100644
index 0000000..f69647d
--- /dev/null
+++ b/src/deploy/pathplanner/paths/ 8 Point to ACTUALMiddleBallPit.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.164464285714287,
+        "y": 5.103964285714285
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.2832380952380955,
+        "y": 4.466904761904762
+      },
+      "isLocked": false,
+      "linkedName": "8Point"
+    },
+    {
+      "anchor": {
+        "x": 8.356380952380952,
+        "y": 4.067392857142857
+      },
+      "prevControl": {
+        "x": 7.460178571428572,
+        "y": 3.7974523809523815
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "ACTUALMiddleBP"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.0,
+    "rotation": -62.681100489851225
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 119.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/ LT Corner 3 to MidBPTL (2).path b/src/deploy/pathplanner/paths/ LT Corner 3 to MidBPTL (2).path
new file mode 100644
index 0000000..048c9cd
--- /dev/null
+++ b/src/deploy/pathplanner/paths/ LT Corner 3 to MidBPTL (2).path	
@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.0655476190476194,
+        "y": 7.069130952380952
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.573035714285715,
+        "y": 7.468642857142857
+      },
+      "isLocked": false,
+      "linkedName": "LTCorner3"
+    },
+    {
+      "anchor": {
+        "x": 7.978464285714287,
+        "y": 4.672059523809524
+      },
+      "prevControl": {
+        "x": 7.514166666665588,
+        "y": 8.192083333333303
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "MidBPTL 2"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.49045643153526897,
+      "rotationDegrees": -126.88328864168005
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 105.00000000000001
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": -71.565051177078
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/ MS - RTBump.path b/src/deploy/pathplanner/paths/ MS - RTBump.path
new file mode 100644
index 0000000..be8ef5d
--- /dev/null
+++ b/src/deploy/pathplanner/paths/ MS - RTBump.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 4.027441666673263
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.8280000000000007,
+        "y": 3.9162261904761904
+      },
+      "isLocked": false,
+      "linkedName": "MS26"
+    },
+    {
+      "anchor": {
+        "x": 3.605428571428572,
+        "y": 2.7932738095238094
+      },
+      "prevControl": {
+        "x": 2.8927857142888667,
+        "y": 2.79327380952119
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "RTBump"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 2.6,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "MS Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/ RT Corner 3 - MidBPBR (2).path b/src/deploy/pathplanner/paths/ RT Corner 3 - MidBPBR (2).path
new file mode 100644
index 0000000..69f197e
--- /dev/null
+++ b/src/deploy/pathplanner/paths/ RT Corner 3 - MidBPBR (2).path	
@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.2491071428625617,
+        "y": 0.7417261904740291
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.177702380952382,
+        "y": 0.5797619047619049
+      },
+      "isLocked": false,
+      "linkedName": "RTCorner3"
+    },
+    {
+      "anchor": {
+        "x": 8.021654761904763,
+        "y": 3.322357142857143
+      },
+      "prevControl": {
+        "x": 7.557357142857144,
+        "y": 0.05067857142857202
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5319502074688782,
+      "rotationDegrees": -158.405945764474
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -105.00000000000001
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 68.19859051479783
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/8Point to MidBPTL.path b/src/deploy/pathplanner/paths/8Point to MidBPTL.path
new file mode 100644
index 0000000..3532b56
--- /dev/null
+++ b/src/deploy/pathplanner/paths/8Point to MidBPTL.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.164464285714287,
+        "y": 5.103964285714285
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.946961768229882,
+        "y": 5.051972035264406
+      },
+      "isLocked": false,
+      "linkedName": "8Point"
+    },
+    {
+      "anchor": {
+        "x": 8.108035714285714,
+        "y": 4.942
+      },
+      "prevControl": {
+        "x": 6.893891929468307,
+        "y": 5.082595602975683
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "MidBPTL"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 105.00000000000001
+  },
+  "reversed": false,
+  "folder": "LT Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 119.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/ACTUALMiddle to RTBump.path b/src/deploy/pathplanner/paths/ACTUALMiddle to RTBump.path
new file mode 100644
index 0000000..b73a335
--- /dev/null
+++ b/src/deploy/pathplanner/paths/ACTUALMiddle to RTBump.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.356380952380952,
+        "y": 4.067392857142857
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.578952380952382,
+        "y": 3.106404761904762
+      },
+      "isLocked": false,
+      "linkedName": "ACTUALMiddleBP"
+    },
+    {
+      "anchor": {
+        "x": 3.605428571428572,
+        "y": 2.7932738095238094
+      },
+      "prevControl": {
+        "x": 4.771571428574581,
+        "y": 1.6919166666640488
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "RTBump"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 1.0,
+    "rotation": -62.681100489851225
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/ACTUALMiddleBallPit to BPTL.path b/src/deploy/pathplanner/paths/ACTUALMiddleBallPit to BPTL.path
new file mode 100644
index 0000000..0dce260
--- /dev/null
+++ b/src/deploy/pathplanner/paths/ACTUALMiddleBallPit to BPTL.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.356380952380952,
+        "y": 4.067392857142857
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.777488095238096,
+        "y": 5.017583333333334
+      },
+      "isLocked": false,
+      "linkedName": "ACTUALMiddleBP"
+    },
+    {
+      "anchor": {
+        "x": 8.291595238095239,
+        "y": 6.734404761908865
+      },
+      "prevControl": {
+        "x": 8.497922053405773,
+        "y": 6.593233783008056
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Rotated BPTL"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 5.2,
+    "rotation": 80.0
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 1.0,
+    "rotation": -62.681100489851225
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/ACTUALMiddleBallPit to LT.path b/src/deploy/pathplanner/paths/ACTUALMiddleBallPit to LT.path
new file mode 100644
index 0000000..7118995
--- /dev/null
+++ b/src/deploy/pathplanner/paths/ACTUALMiddleBallPit to LT.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.291595238095239,
+        "y": 4.035
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.129630952380953,
+        "y": 7.403857142857143
+      },
+      "isLocked": false,
+      "linkedName": "ACTUALMiddleBPversion2"
+    },
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 7.382261904761905
+      },
+      "prevControl": {
+        "x": 4.782369047619048,
+        "y": 7.7709761904761905
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "LT26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 77.829
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/ACTUALMiddleBallPit to LTBump.path b/src/deploy/pathplanner/paths/ACTUALMiddleBallPit to LTBump.path
new file mode 100644
index 0000000..7f3b857
--- /dev/null
+++ b/src/deploy/pathplanner/paths/ACTUALMiddleBallPit to LTBump.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.291595238095239,
+        "y": 4.035
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.812321428571429,
+        "y": 5.222738095238095
+      },
+      "isLocked": false,
+      "linkedName": "ACTUALMiddleBPversion2"
+    },
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 5.222738095238095
+      },
+      "prevControl": {
+        "x": 5.387035714286508,
+        "y": 5.384702380956204
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "LTBump26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -0.0
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 1.0,
+    "rotation": 77.829
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/ACTUALMiddleBallPit to RT.path b/src/deploy/pathplanner/paths/ACTUALMiddleBallPit to RT.path
new file mode 100644
index 0000000..defb7f7
--- /dev/null
+++ b/src/deploy/pathplanner/paths/ACTUALMiddleBallPit to RT.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.356380952380952,
+        "y": 4.067392857142857
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.96104761904762,
+        "y": 1.173630952380953
+      },
+      "isLocked": false,
+      "linkedName": "ACTUALMiddleBP"
+    },
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 0.6445476190476187
+      },
+      "prevControl": {
+        "x": 5.333047619047619,
+        "y": 0.09386904761904813
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "RT26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -62.681100489851225
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/ACTUALMiddleBallPit2 to BPBR.path b/src/deploy/pathplanner/paths/ACTUALMiddleBallPit2 to BPBR.path
new file mode 100644
index 0000000..3e44d69
--- /dev/null
+++ b/src/deploy/pathplanner/paths/ACTUALMiddleBallPit2 to BPBR.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.291595238095239,
+        "y": 4.035
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 9.291595238095239,
+        "y": 4.035
+      },
+      "isLocked": false,
+      "linkedName": "ACTUALMiddleBPversion2"
+    },
+    {
+      "anchor": {
+        "x": 8.356380952380952,
+        "y": 1.4111785714285725
+      },
+      "prevControl": {
+        "x": 8.647916666666667,
+        "y": 1.4543690476190476
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "BPBR26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.0,
+    "rotation": 80.0
+  },
+  "reversed": false,
+  "folder": "RT Paths",
+  "idealStartingState": {
+    "velocity": 1.0,
+    "rotation": 77.829
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/BPBR - RTBump.path b/src/deploy/pathplanner/paths/BPBR - RTBump.path
new file mode 100644
index 0000000..9ed2be6
--- /dev/null
+++ b/src/deploy/pathplanner/paths/BPBR - RTBump.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.291595238095239,
+        "y": 1.367988095238095
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.52524399478248,
+        "y": 1.2790597055163297
+      },
+      "isLocked": false,
+      "linkedName": "Rotated BPBR"
+    },
+    {
+      "anchor": {
+        "x": 3.605428571428572,
+        "y": 2.7932738095238094
+      },
+      "prevControl": {
+        "x": 2.498860371158594,
+        "y": 2.7142288388105023
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "RTBump"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.5,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "RT Paths",
+  "idealStartingState": {
+    "velocity": 5.2,
+    "rotation": -80.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/BPBR to MiddleBallPit.path b/src/deploy/pathplanner/paths/BPBR to MiddleBallPit.path
new file mode 100644
index 0000000..cda8d25
--- /dev/null
+++ b/src/deploy/pathplanner/paths/BPBR to MiddleBallPit.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.291595238095239,
+        "y": 1.367988095238095
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.50754761904762,
+        "y": 1.8646785714285707
+      },
+      "isLocked": false,
+      "linkedName": "Rotated BPBR"
+    },
+    {
+      "anchor": {
+        "x": 8.356380952380952,
+        "y": 4.067392857142857
+      },
+      "prevControl": {
+        "x": 8.410369047619048,
+        "y": 3.333154761904763
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "ACTUALMiddleBP"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.0,
+    "rotation": -62.681100489851225
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -80.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/BPBR to RT.path b/src/deploy/pathplanner/paths/BPBR to RT.path
new file mode 100644
index 0000000..14e5aa0
--- /dev/null
+++ b/src/deploy/pathplanner/paths/BPBR to RT.path	
@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.356380952380952,
+        "y": 1.4111785714285725
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.194416666666667,
+        "y": 1.1304404761904754
+      },
+      "isLocked": false,
+      "linkedName": "BPBR26"
+    },
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 0.6445476190476187
+      },
+      "prevControl": {
+        "x": 5.354642857142514,
+        "y": 0.5257738095171476
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "RT26"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.85,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Return to Alliance",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 80.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/BPBR to RTBump.path b/src/deploy/pathplanner/paths/BPBR to RTBump.path
new file mode 100644
index 0000000..21c7884
--- /dev/null
+++ b/src/deploy/pathplanner/paths/BPBR to RTBump.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.356380952380952,
+        "y": 1.4111785714285725
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.854063306316862,
+        "y": 1.8650666541486076
+      },
+      "isLocked": false,
+      "linkedName": "BPBR26"
+    },
+    {
+      "anchor": {
+        "x": 3.605428571428572,
+        "y": 2.7932738095238094
+      },
+      "prevControl": {
+        "x": 3.2808532809441355,
+        "y": 2.8952075668049244
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "RTBump"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 2.6,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Return to Alliance",
+  "idealStartingState": {
+    "velocity": 5.2,
+    "rotation": 80.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/BPBR to RTCorner3.path b/src/deploy/pathplanner/paths/BPBR to RTCorner3.path
new file mode 100644
index 0000000..166022d
--- /dev/null
+++ b/src/deploy/pathplanner/paths/BPBR to RTCorner3.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.356380952380952,
+        "y": 1.4111785714285725
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.736738095238096,
+        "y": 0.14785714285714335
+      },
+      "isLocked": false,
+      "linkedName": "BPBR26"
+    },
+    {
+      "anchor": {
+        "x": 3.2491071428625617,
+        "y": 0.7417261904740291
+      },
+      "prevControl": {
+        "x": 3.51904761904762,
+        "y": 0.6877380952380951
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "RTCorner3"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 68.19859051479783
+  },
+  "reversed": false,
+  "folder": "RT Paths",
+  "idealStartingState": {
+    "velocity": 1.0,
+    "rotation": 80.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/BPTL to ACTUALMiddleversion2.path b/src/deploy/pathplanner/paths/BPTL to ACTUALMiddleversion2.path
new file mode 100644
index 0000000..8dc9e41
--- /dev/null
+++ b/src/deploy/pathplanner/paths/BPTL to ACTUALMiddleversion2.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.291595238095239,
+        "y": 6.734404761908865
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.647916666666667,
+        "y": 5.4710833333333335
+      },
+      "isLocked": false,
+      "linkedName": "Rotated BPTL"
+    },
+    {
+      "anchor": {
+        "x": 8.291595238095239,
+        "y": 4.035
+      },
+      "prevControl": {
+        "x": 8.572333333333333,
+        "y": 4.758440476190476
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "ACTUALMiddleBPversion2"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.0,
+    "rotation": 77.829
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 80.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/BPTL to LT.path b/src/deploy/pathplanner/paths/BPTL to LT.path
new file mode 100644
index 0000000..95183a6
--- /dev/null
+++ b/src/deploy/pathplanner/paths/BPTL to LT.path	
@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.356380952380952,
+        "y": 6.604833333333334
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.924476190476192,
+        "y": 7.036738095238095
+      },
+      "isLocked": false,
+      "linkedName": "BPTL26"
+    },
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 7.382261904761905
+      },
+      "prevControl": {
+        "x": 6.0024999999999995,
+        "y": 7.79257142857143
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "LT26"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.8038379530916856,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Return to Alliance",
+  "idealStartingState": {
+    "velocity": 5.2,
+    "rotation": 70.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/Bottom to Rotated Top.path b/src/deploy/pathplanner/paths/Bottom to Rotated Top.path
new file mode 100644
index 0000000..3b41c3a
--- /dev/null
+++ b/src/deploy/pathplanner/paths/Bottom to Rotated Top.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.291595238095239,
+        "y": 1.367988095238095
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.550856438640444,
+        "y": 2.1199160965079384
+      },
+      "isLocked": false,
+      "linkedName": "Rotated BPBR"
+    },
+    {
+      "anchor": {
+        "x": 8.356380952380952,
+        "y": 6.604833333333334
+      },
+      "prevControl": {
+        "x": 8.637119047619048,
+        "y": 5.319916666666667
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "BPTL26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 5.2,
+    "rotation": 70.0
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 5.2,
+    "rotation": -80.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/Bottom to Top.path b/src/deploy/pathplanner/paths/Bottom to Top.path
new file mode 100644
index 0000000..1868906
--- /dev/null
+++ b/src/deploy/pathplanner/paths/Bottom to Top.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.291595238095239,
+        "y": 1.367988095238095
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.550856438640444,
+        "y": 2.1199160965079384
+      },
+      "isLocked": false,
+      "linkedName": "Rotated BPBR"
+    },
+    {
+      "anchor": {
+        "x": 8.291595238095239,
+        "y": 6.734404761908865
+      },
+      "prevControl": {
+        "x": 8.572333333333335,
+        "y": 5.449488095242199
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Rotated BPTL"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 5.2,
+    "rotation": 80.0
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 5.2,
+    "rotation": -80.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/CornerLine LT to DP.path b/src/deploy/pathplanner/paths/CornerLine LT to DP.path
new file mode 100644
index 0000000..5cd25ac
--- /dev/null
+++ b/src/deploy/pathplanner/paths/CornerLine LT to DP.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5622380952380954,
+        "y": 7.252690476190476
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0854071663965237,
+        "y": 6.8510872698347525
+      },
+      "isLocked": false,
+      "linkedName": "LTedge"
+    },
+    {
+      "anchor": {
+        "x": 0.9816071428571436,
+        "y": 5.902988095238095
+      },
+      "prevControl": {
+        "x": 3.7063552838449105,
+        "y": 6.4144083322574375
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "DP26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "LT Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 138.01278750418322
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/DP to Climb.path b/src/deploy/pathplanner/paths/DP to Climb.path
new file mode 100644
index 0000000..7c47340
--- /dev/null
+++ b/src/deploy/pathplanner/paths/DP to Climb.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.9816071428571436,
+        "y": 5.902988095238095
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.9425952380952385,
+        "y": 5.535869047619047
+      },
+      "isLocked": false,
+      "linkedName": "DP26"
+    },
+    {
+      "anchor": {
+        "x": 1.391916666672215,
+        "y": 3.7218690476211576
+      },
+      "prevControl": {
+        "x": 2.028976190476192,
+        "y": 4.391321428571429
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Climb26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "RT Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/DP to SUTO.path b/src/deploy/pathplanner/paths/DP to SUTO.path
new file mode 100644
index 0000000..0fab458
--- /dev/null
+++ b/src/deploy/pathplanner/paths/DP to SUTO.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.9816071428571436,
+        "y": 5.902988095238095
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.2966160836915332,
+        "y": 5.473779440328829
+      },
+      "isLocked": false,
+      "linkedName": "DP26"
+    },
+    {
+      "anchor": {
+        "x": 2.439285714285715,
+        "y": 4.026361904761904
+      },
+      "prevControl": {
+        "x": 2.2808854900162623,
+        "y": 4.274460110649111
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "SUTO26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Return to Alliance",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/Fadeaway Top to Bottom.path b/src/deploy/pathplanner/paths/Fadeaway Top to Bottom.path
new file mode 100644
index 0000000..87704d8
--- /dev/null
+++ b/src/deploy/pathplanner/paths/Fadeaway Top to Bottom.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0721666666666674,
+        "y": 6.496857142857143
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.6402619047619056,
+        "y": 4.801630952380952
+      },
+      "isLocked": false,
+      "linkedName": "PeakSuto26"
+    },
+    {
+      "anchor": {
+        "x": 2.514869047619049,
+        "y": 0.7525238095238092
+      },
+      "prevControl": {
+        "x": 1.5970714285714296,
+        "y": 2.760880952380953
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 3.0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 3.0,
+    "rotation": -56.7987924497313
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/LT - MidBPTL.path b/src/deploy/pathplanner/paths/LT - MidBPTL.path
new file mode 100644
index 0000000..e2330c4
--- /dev/null
+++ b/src/deploy/pathplanner/paths/LT - MidBPTL.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 7.382261904761905
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.814761904761906,
+        "y": 7.706190476190477
+      },
+      "isLocked": false,
+      "linkedName": "LT26"
+    },
+    {
+      "anchor": {
+        "x": 8.108035714285714,
+        "y": 4.942
+      },
+      "prevControl": {
+        "x": 8.108035714285714,
+        "y": 7.587416666666666
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "MidBPTL"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 105.00000000000001
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/LT Corner 3 to 8Point.path b/src/deploy/pathplanner/paths/LT Corner 3 to 8Point.path
new file mode 100644
index 0000000..ee3ae46
--- /dev/null
+++ b/src/deploy/pathplanner/paths/LT Corner 3 to 8Point.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.0655476190476194,
+        "y": 7.069130952380952
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.5622380952380954,
+        "y": 7.976130952380952
+      },
+      "isLocked": false,
+      "linkedName": "LTCorner3"
+    },
+    {
+      "anchor": {
+        "x": 6.164464285714287,
+        "y": 5.103964285714285
+      },
+      "prevControl": {
+        "x": 7.2442261904761915,
+        "y": 7.652202380952382
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "8Point"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.0,
+    "rotation": 119.99999999999999
+  },
+  "reversed": false,
+  "folder": "LT Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -71.565051177078
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/LT Corner 3 to DP.path b/src/deploy/pathplanner/paths/LT Corner 3 to DP.path
new file mode 100644
index 0000000..02965b6
--- /dev/null
+++ b/src/deploy/pathplanner/paths/LT Corner 3 to DP.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.0655476190476194,
+        "y": 7.069130952380952
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.7180840978146676,
+        "y": 6.557127768372483
+      },
+      "isLocked": false,
+      "linkedName": "LTCorner3"
+    },
+    {
+      "anchor": {
+        "x": 0.9816071428571436,
+        "y": 5.902988095238095
+      },
+      "prevControl": {
+        "x": 1.99534891528081,
+        "y": 6.048770164765715
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "DP26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "LT Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -71.565051177078
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/LT Corner 3 to MidBPTL (Sped Up).path b/src/deploy/pathplanner/paths/LT Corner 3 to MidBPTL (Sped Up).path
new file mode 100644
index 0000000..926277b
--- /dev/null
+++ b/src/deploy/pathplanner/paths/LT Corner 3 to MidBPTL (Sped Up).path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.0655476190476194,
+        "y": 7.069130952380952
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.573035714285715,
+        "y": 7.468642857142857
+      },
+      "isLocked": false,
+      "linkedName": "LTCorner3"
+    },
+    {
+      "anchor": {
+        "x": 8.108035714285714,
+        "y": 4.942
+      },
+      "prevControl": {
+        "x": 6.844714285714285,
+        "y": 7.490238095238096
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "MidBPTL"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 105.00000000000001
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 5.2,
+    "rotation": -71.565051177078
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/LT Corner 3 to MidBPTL.path b/src/deploy/pathplanner/paths/LT Corner 3 to MidBPTL.path
new file mode 100644
index 0000000..ec13c1a
--- /dev/null
+++ b/src/deploy/pathplanner/paths/LT Corner 3 to MidBPTL.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.0655476190476194,
+        "y": 7.069130952380952
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.573035714285715,
+        "y": 7.468642857142857
+      },
+      "isLocked": false,
+      "linkedName": "LTCorner3"
+    },
+    {
+      "anchor": {
+        "x": 8.108035714285714,
+        "y": 4.942
+      },
+      "prevControl": {
+        "x": 6.844714285714284,
+        "y": 7.490238095238097
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "MidBPTL"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 105.00000000000001
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": -71.565051177078
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/LT Corner 3 to SUTO.path b/src/deploy/pathplanner/paths/LT Corner 3 to SUTO.path
new file mode 100644
index 0000000..d1b7b8a
--- /dev/null
+++ b/src/deploy/pathplanner/paths/LT Corner 3 to SUTO.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.0655476190476194,
+        "y": 7.069130952380952
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.6315601025213216,
+        "y": 6.112181050721057
+      },
+      "isLocked": false,
+      "linkedName": "LTCorner3"
+    },
+    {
+      "anchor": {
+        "x": 2.439285714285715,
+        "y": 4.026361904761904
+      },
+      "prevControl": {
+        "x": 2.4025084771978844,
+        "y": 3.7638721311844447
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "SUTO26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -71.565051177078
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/LT CornerLine to SUTO.path b/src/deploy/pathplanner/paths/LT CornerLine to SUTO.path
new file mode 100644
index 0000000..2b5dcad
--- /dev/null
+++ b/src/deploy/pathplanner/paths/LT CornerLine to SUTO.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5622380952380954,
+        "y": 7.252690476190476
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.7848095238095247,
+        "y": 5.892190476190476
+      },
+      "isLocked": false,
+      "linkedName": "LTedge"
+    },
+    {
+      "anchor": {
+        "x": 2.439285714285715,
+        "y": 4.026361904761904
+      },
+      "prevControl": {
+        "x": 2.7092261904761914,
+        "y": 5.1579523809523815
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "SUTO26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "LT Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 138.01278750418322
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/LT To PeakSUTO.path b/src/deploy/pathplanner/paths/LT To PeakSUTO.path
new file mode 100644
index 0000000..2cd065e
--- /dev/null
+++ b/src/deploy/pathplanner/paths/LT To PeakSUTO.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 7.382261904761905
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0655476190476194,
+        "y": 7.231095238095239
+      },
+      "isLocked": false,
+      "linkedName": "LT26"
+    },
+    {
+      "anchor": {
+        "x": 2.0721666666666674,
+        "y": 6.496857142857143
+      },
+      "prevControl": {
+        "x": 2.406892857142858,
+        "y": 6.6912142857142864
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "PeakSuto26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -56.7987924497313
+  },
+  "reversed": false,
+  "folder": "LT Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/LT to BPTL.path b/src/deploy/pathplanner/paths/LT to BPTL.path
new file mode 100644
index 0000000..7110984
--- /dev/null
+++ b/src/deploy/pathplanner/paths/LT to BPTL.path	
@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 7.382261904761905
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.6162261904761905,
+        "y": 7.382261904761905
+      },
+      "isLocked": false,
+      "linkedName": "LT26"
+    },
+    {
+      "anchor": {
+        "x": 8.356380952380952,
+        "y": 6.604833333333334
+      },
+      "prevControl": {
+        "x": 8.259457861635237,
+        "y": 7.774570498385188
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "BPTL26"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.7,
+      "rotationDegrees": 80.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 5.2,
+    "rotation": 70.0
+  },
+  "reversed": false,
+  "folder": "LT Paths",
+  "idealStartingState": {
+    "velocity": 2.6,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/LT to Climb.path b/src/deploy/pathplanner/paths/LT to Climb.path
new file mode 100644
index 0000000..9dd93a7
--- /dev/null
+++ b/src/deploy/pathplanner/paths/LT to Climb.path	
@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 7.382261904761905
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.427376794362186,
+        "y": 7.1259103257731775
+      },
+      "isLocked": false,
+      "linkedName": "LT26"
+    },
+    {
+      "anchor": {
+        "x": 1.391916666672215,
+        "y": 3.7218690476211576
+      },
+      "prevControl": {
+        "x": 1.5849704671728562,
+        "y": 4.024196164168285
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Climb26"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.67,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "LT Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/LT to DP.path b/src/deploy/pathplanner/paths/LT to DP.path
new file mode 100644
index 0000000..97b7a3b
--- /dev/null
+++ b/src/deploy/pathplanner/paths/LT to DP.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 7.382261904761905
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.139395261634619,
+        "y": 6.980658698406182
+      },
+      "isLocked": false,
+      "linkedName": "LT26"
+    },
+    {
+      "anchor": {
+        "x": 0.9816071428571436,
+        "y": 5.902988095238095
+      },
+      "prevControl": {
+        "x": 3.7063552838449105,
+        "y": 6.4144083322574375
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "DP26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "LT Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/LT to LTBump26.path b/src/deploy/pathplanner/paths/LT to LTBump26.path
new file mode 100644
index 0000000..b61aeee
--- /dev/null
+++ b/src/deploy/pathplanner/paths/LT to LTBump26.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 7.382261904761905
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0115595238095243,
+        "y": 7.058333333333334
+      },
+      "isLocked": false,
+      "linkedName": "LT26"
+    },
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 5.222738095238095
+      },
+      "prevControl": {
+        "x": 3.4237790301037356,
+        "y": 5.382312812736133
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "LTBump26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -0.0
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/LT to OP.path b/src/deploy/pathplanner/paths/LT to OP.path
new file mode 100644
index 0000000..64edbe0
--- /dev/null
+++ b/src/deploy/pathplanner/paths/LT to OP.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 7.382261904761905
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.40027380952381,
+        "y": 6.313297619047619
+      },
+      "isLocked": false,
+      "linkedName": "LT26"
+    },
+    {
+      "anchor": {
+        "x": 0.290559523803116,
+        "y": 0.6445476190433237
+      },
+      "prevControl": {
+        "x": 1.160068443968153,
+        "y": 0.916054929396755
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "OP26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "LT Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/LT to Rotated BPTL.path b/src/deploy/pathplanner/paths/LT to Rotated BPTL.path
new file mode 100644
index 0000000..6c52a88
--- /dev/null
+++ b/src/deploy/pathplanner/paths/LT to Rotated BPTL.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 7.382261904761905
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.6162261904761905,
+        "y": 7.382261904761905
+      },
+      "isLocked": false,
+      "linkedName": "LT26"
+    },
+    {
+      "anchor": {
+        "x": 8.291595238095239,
+        "y": 6.734404761908865
+      },
+      "prevControl": {
+        "x": 8.097238095238097,
+        "y": 7.868154761904762
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Rotated BPTL"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 5.2,
+    "rotation": 80.0
+  },
+  "reversed": false,
+  "folder": "LT Paths",
+  "idealStartingState": {
+    "velocity": 2.6,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/LT to Shoot.path b/src/deploy/pathplanner/paths/LT to Shoot.path
new file mode 100644
index 0000000..c8e7969
--- /dev/null
+++ b/src/deploy/pathplanner/paths/LT to Shoot.path	
@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 7.382261904761905
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.357083333333334,
+        "y": 7.231095238095239
+      },
+      "isLocked": false,
+      "linkedName": "LT26"
+    },
+    {
+      "anchor": {
+        "x": 2.439285714285715,
+        "y": 4.026361904761904
+      },
+      "prevControl": {
+        "x": 1.9641904761904772,
+        "y": 4.682857142857142
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "SUTO26"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.6763485477178376,
+      "rotationDegrees": 38.21198407992096
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "LT Paths",
+  "idealStartingState": {
+    "velocity": 5.0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/LTBump - BPTL.path b/src/deploy/pathplanner/paths/LTBump - BPTL.path
new file mode 100644
index 0000000..ea88c4d
--- /dev/null
+++ b/src/deploy/pathplanner/paths/LTBump - BPTL.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 5.222738095238095
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.48614545814791,
+        "y": 5.7824488361109765
+      },
+      "isLocked": false,
+      "linkedName": "LTBump26"
+    },
+    {
+      "anchor": {
+        "x": 8.356380952380952,
+        "y": 6.604833333333334
+      },
+      "prevControl": {
+        "x": 6.671952380952382,
+        "y": 7.339071428571429
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "BPTL26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 5.2,
+    "rotation": 70.0
+  },
+  "reversed": false,
+  "folder": "LT Paths",
+  "idealStartingState": {
+    "velocity": 2.0,
+    "rotation": -0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/LTBump to DP.path b/src/deploy/pathplanner/paths/LTBump to DP.path
new file mode 100644
index 0000000..e8b17ba
--- /dev/null
+++ b/src/deploy/pathplanner/paths/LTBump to DP.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 5.222738095238095
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.7673326941719103,
+        "y": 5.449956083628163
+      },
+      "isLocked": false,
+      "linkedName": "LTBump26"
+    },
+    {
+      "anchor": {
+        "x": 0.9816071428571436,
+        "y": 5.902988095238095
+      },
+      "prevControl": {
+        "x": 1.3379355911939546,
+        "y": 5.827090923753036
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "DP26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/LTBump to LTCorner 3.path b/src/deploy/pathplanner/paths/LTBump to LTCorner 3.path
new file mode 100644
index 0000000..bbfcf63
--- /dev/null
+++ b/src/deploy/pathplanner/paths/LTBump to LTCorner 3.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 5.222738095238095
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.3448672731329427,
+        "y": 6.236235526336788
+      },
+      "isLocked": false,
+      "linkedName": "LTBump26"
+    },
+    {
+      "anchor": {
+        "x": 3.0655476190476194,
+        "y": 7.069130952380952
+      },
+      "prevControl": {
+        "x": 3.179513126031883,
+        "y": 6.622059392887153
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "LTCorner3"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -71.565051177078
+  },
+  "reversed": false,
+  "folder": "LT Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/LTBump to SUTO.path b/src/deploy/pathplanner/paths/LTBump to SUTO.path
new file mode 100644
index 0000000..dc18875
--- /dev/null
+++ b/src/deploy/pathplanner/paths/LTBump to SUTO.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 5.222738095238095
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.8387976190484125,
+        "y": 5.2119404761942985
+      },
+      "isLocked": false,
+      "linkedName": "LTBump26"
+    },
+    {
+      "anchor": {
+        "x": 2.439285714285715,
+        "y": 4.026361904761904
+      },
+      "prevControl": {
+        "x": 2.2233333333333345,
+        "y": 4.240154761904762
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "SUTO26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "LT Paths",
+  "idealStartingState": {
+    "velocity": 2.6,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/MS - LT Corner 3.path b/src/deploy/pathplanner/paths/MS - LT Corner 3.path
new file mode 100644
index 0000000..2dcbe77
--- /dev/null
+++ b/src/deploy/pathplanner/paths/MS - LT Corner 3.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 4.027441666673263
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.1003809523809531,
+        "y": 4.769238095238096
+      },
+      "isLocked": false,
+      "linkedName": "MS26"
+    },
+    {
+      "anchor": {
+        "x": 3.0655476190476194,
+        "y": 7.069130952380952
+      },
+      "prevControl": {
+        "x": 2.2773214285714296,
+        "y": 6.971952380952381
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "LTCorner3"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 5.2,
+    "rotation": -71.565051177078
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/MS - LTBump.path b/src/deploy/pathplanner/paths/MS - LTBump.path
new file mode 100644
index 0000000..304c70a
--- /dev/null
+++ b/src/deploy/pathplanner/paths/MS - LTBump.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 4.027441666673263
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.8603928571428576,
+        "y": 4.229357142857142
+      },
+      "isLocked": false,
+      "linkedName": "MS26"
+    },
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 5.222738095238095
+      },
+      "prevControl": {
+        "x": 2.9251785714293654,
+        "y": 5.190345238099061
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "LTBump26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 2.6,
+    "rotation": -0.0
+  },
+  "reversed": false,
+  "folder": "MS Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/MS - RT Corner 3.path b/src/deploy/pathplanner/paths/MS - RT Corner 3.path
new file mode 100644
index 0000000..739fb48
--- /dev/null
+++ b/src/deploy/pathplanner/paths/MS - RT Corner 3.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 4.027441666673263
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.1693452380952394,
+        "y": 3.8298452380952384
+      },
+      "isLocked": false,
+      "linkedName": "MS26"
+    },
+    {
+      "anchor": {
+        "x": 3.2491071428625617,
+        "y": 0.7417261904740291
+      },
+      "prevControl": {
+        "x": 1.6510595238095254,
+        "y": 1.5407500000000005
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "RTCorner3"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 5.2,
+    "rotation": 68.19859051479783
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/MS to Climb.path b/src/deploy/pathplanner/paths/MS to Climb.path
new file mode 100644
index 0000000..035766c
--- /dev/null
+++ b/src/deploy/pathplanner/paths/MS to Climb.path	
@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 4.027441666673263
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.0521851087532084,
+        "y": 3.813880363816242
+      },
+      "isLocked": false,
+      "linkedName": "MS26"
+    },
+    {
+      "anchor": {
+        "x": 1.391916666672215,
+        "y": 3.7218690476211576
+      },
+      "prevControl": {
+        "x": 2.7334323769229343,
+        "y": 3.9027427300925375
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Climb26"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.42738589211618644,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "MS Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/MS to DP.path b/src/deploy/pathplanner/paths/MS to DP.path
new file mode 100644
index 0000000..531cb06
--- /dev/null
+++ b/src/deploy/pathplanner/paths/MS to DP.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 4.027441666673263
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.8769698196665643,
+        "y": 4.022246282787717
+      },
+      "isLocked": false,
+      "linkedName": "MS26"
+    },
+    {
+      "anchor": {
+        "x": 0.9816071428571436,
+        "y": 5.902988095238095
+      },
+      "prevControl": {
+        "x": 1.8943474552851443,
+        "y": 5.106192184801821
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "DP26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "MS Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/MS to LT.path b/src/deploy/pathplanner/paths/MS to LT.path
new file mode 100644
index 0000000..42f9d36
--- /dev/null
+++ b/src/deploy/pathplanner/paths/MS to LT.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 4.027441666673263
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.9510805614963314,
+        "y": 5.223127528845973
+      },
+      "isLocked": false,
+      "linkedName": "MS26"
+    },
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 7.382261904761905
+      },
+      "prevControl": {
+        "x": 1.7914285714285718,
+        "y": 7.079928571428571
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "LT26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 2.6,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "MS Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/MS to OP.path b/src/deploy/pathplanner/paths/MS to OP.path
new file mode 100644
index 0000000..7569fc9
--- /dev/null
+++ b/src/deploy/pathplanner/paths/MS to OP.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 4.027441666673263
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.6029186747389828,
+        "y": 2.97048547998539
+      },
+      "isLocked": false,
+      "linkedName": "MS26"
+    },
+    {
+      "anchor": {
+        "x": 0.290559523803116,
+        "y": 0.6445476190433237
+      },
+      "prevControl": {
+        "x": 1.1835145259329864,
+        "y": 1.537530143908434
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "OP26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "MS Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/MS to RT.path b/src/deploy/pathplanner/paths/MS to RT.path
new file mode 100644
index 0000000..7065a6f
--- /dev/null
+++ b/src/deploy/pathplanner/paths/MS to RT.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 4.027441666673263
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.3786785714285723,
+        "y": 3.754261904761905
+      },
+      "isLocked": false,
+      "linkedName": "MS26"
+    },
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 0.6445476190476187
+      },
+      "prevControl": {
+        "x": 1.8346190476190485,
+        "y": 0.8497023809523808
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "RT26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 2.6,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "MS Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/MidBPBR - RT Corner 3.path b/src/deploy/pathplanner/paths/MidBPBR - RT Corner 3.path
new file mode 100644
index 0000000..2e36eff
--- /dev/null
+++ b/src/deploy/pathplanner/paths/MidBPBR - RT Corner 3.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.151226190476192,
+        "y": 3.322357142857143
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.05404761904762,
+        "y": 0.4825833333333325
+      },
+      "isLocked": false,
+      "linkedName": "MidBPBR"
+    },
+    {
+      "anchor": {
+        "x": 3.2491071428625617,
+        "y": 0.7417261904740291
+      },
+      "prevControl": {
+        "x": 3.486654761904763,
+        "y": 0.3206190476190477
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "RTCorner3"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 68.19859051479783
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -105.00000000000001
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/MidBPTL - LT Corner 3.path b/src/deploy/pathplanner/paths/MidBPTL - LT Corner 3.path
new file mode 100644
index 0000000..ef8ded4
--- /dev/null
+++ b/src/deploy/pathplanner/paths/MidBPTL - LT Corner 3.path	
@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.108035714285714,
+        "y": 4.942
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.892083333333334,
+        "y": 6.637226190476191
+      },
+      "isLocked": false,
+      "linkedName": "MidBPTL"
+    },
+    {
+      "anchor": {
+        "x": 3.0655476190476194,
+        "y": 7.069130952380952
+      },
+      "prevControl": {
+        "x": 5.387035714285714,
+        "y": 8.13809523809524
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "LTCorner3"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.712863070539417,
+      "rotationDegrees": -111.29596569181103
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -71.565051177078
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 105.00000000000001
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/OP to SUTO.path b/src/deploy/pathplanner/paths/OP to SUTO.path
new file mode 100644
index 0000000..ff7fbae
--- /dev/null
+++ b/src/deploy/pathplanner/paths/OP to SUTO.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.290559523803116,
+        "y": 0.6445476190433237
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 0.6864808039026886,
+        "y": 1.2760850528715435
+      },
+      "isLocked": false,
+      "linkedName": "OP26"
+    },
+    {
+      "anchor": {
+        "x": 2.439285714285715,
+        "y": 4.026361904761904
+      },
+      "prevControl": {
+        "x": 2.2147010365132296,
+        "y": 3.653347012852909
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "SUTO26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Return to Alliance",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RT - MidBPBR.path b/src/deploy/pathplanner/paths/RT - MidBPBR.path
new file mode 100644
index 0000000..8750f9c
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RT - MidBPBR.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 0.6445476190476187
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.326428571428572,
+        "y": 0.029083333333334127
+      },
+      "isLocked": false,
+      "linkedName": "RT26"
+    },
+    {
+      "anchor": {
+        "x": 8.151226190476192,
+        "y": 3.322357142857143
+      },
+      "prevControl": {
+        "x": 8.205214285714288,
+        "y": 2.1994047619047628
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "MidBPBR"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -105.00000000000001
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RT - PeakSUTO.path b/src/deploy/pathplanner/paths/RT - PeakSUTO.path
new file mode 100644
index 0000000..036e1e2
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RT - PeakSUTO.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 0.6445476190476187
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.957571428571429,
+        "y": 0.4501904761904758
+      },
+      "isLocked": false,
+      "linkedName": "RT26"
+    },
+    {
+      "anchor": {
+        "x": 2.03977380952381,
+        "y": 1.5191547619047627
+      },
+      "prevControl": {
+        "x": 1.8445566071630546,
+        "y": 1.362981000016154
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "RT PeakSUTO"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 56.8
+  },
+  "reversed": false,
+  "folder": "RT Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RT 8 Point to ACTUALMiddleBallPit2.path b/src/deploy/pathplanner/paths/RT 8 Point to ACTUALMiddleBallPit2.path
new file mode 100644
index 0000000..15bbaf3
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RT 8 Point to ACTUALMiddleBallPit2.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.9701071428571435,
+        "y": 3.3115595238095237
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.877107142857143,
+        "y": 3.6354880952380952
+      },
+      "isLocked": false,
+      "linkedName": "RT 8 Point"
+    },
+    {
+      "anchor": {
+        "x": 8.291595238095239,
+        "y": 4.035
+      },
+      "prevControl": {
+        "x": 7.2766190476190475,
+        "y": 3.7974523809523815
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "ACTUALMiddleBPversion2"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.0,
+    "rotation": 77.829
+  },
+  "reversed": false,
+  "folder": "RT Paths",
+  "idealStartingState": {
+    "velocity": 1.0,
+    "rotation": -119.99999999999999
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RT Corner 3 - MidBPBR (2) Sped Up.path b/src/deploy/pathplanner/paths/RT Corner 3 - MidBPBR (2) Sped Up.path
new file mode 100644
index 0000000..2e5c849
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RT Corner 3 - MidBPBR (2) Sped Up.path	
@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.2491071428625617,
+        "y": 0.7417261904740291
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.177702380952382,
+        "y": 0.5797619047619049
+      },
+      "isLocked": false,
+      "linkedName": "RTCorner3"
+    },
+    {
+      "anchor": {
+        "x": 8.021654761904763,
+        "y": 3.322357142857143
+      },
+      "prevControl": {
+        "x": 7.557357142857144,
+        "y": 0.05067857142857202
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5319502074688782,
+      "rotationDegrees": -158.405945764474
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -105.00000000000001
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 1.5,
+    "rotation": 68.199
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RT Corner 3 - MidBPBR (Sped Up).path b/src/deploy/pathplanner/paths/RT Corner 3 - MidBPBR (Sped Up).path
new file mode 100644
index 0000000..fec36a2
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RT Corner 3 - MidBPBR (Sped Up).path	
@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.2491071428625617,
+        "y": 0.7417261904740291
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.339666666666668,
+        "y": 0.4501904761904754
+      },
+      "isLocked": false,
+      "linkedName": "RTCorner3"
+    },
+    {
+      "anchor": {
+        "x": 8.151226190476192,
+        "y": 3.322357142857143
+      },
+      "prevControl": {
+        "x": 7.589750000000002,
+        "y": 0.9576785714285712
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "MidBPBR"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5,
+      "rotationDegrees": 162.52111793047007
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 3.0,
+    "rotation": -105.00000000000001
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 5.2,
+    "rotation": 68.19859051479783
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RT Corner 3 - MidBPBR.path b/src/deploy/pathplanner/paths/RT Corner 3 - MidBPBR.path
new file mode 100644
index 0000000..9c22d3b
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RT Corner 3 - MidBPBR.path	
@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.2491071428625617,
+        "y": 0.7417261904740291
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.339666666666668,
+        "y": 0.4501904761904754
+      },
+      "isLocked": false,
+      "linkedName": "RTCorner3"
+    },
+    {
+      "anchor": {
+        "x": 8.151226190476192,
+        "y": 3.322357142857143
+      },
+      "prevControl": {
+        "x": 7.589750000000002,
+        "y": 0.9576785714285712
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "MidBPBR"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5,
+      "rotationDegrees": 175.03025927185985
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -105.00000000000001
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 68.19859051479783
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RT Corner 3 ro RT 8 Point.path b/src/deploy/pathplanner/paths/RT Corner 3 ro RT 8 Point.path
new file mode 100644
index 0000000..2ddc6f7
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RT Corner 3 ro RT 8 Point.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.2491071428625617,
+        "y": 0.7417261904740291
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.8969642857142865,
+        "y": 0.01828571428571424
+      },
+      "isLocked": false,
+      "linkedName": "RTCorner3"
+    },
+    {
+      "anchor": {
+        "x": 5.9701071428571435,
+        "y": 3.3115595238095237
+      },
+      "prevControl": {
+        "x": 7.406190476190479,
+        "y": 0.1262619047619027
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "RT 8 Point"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.0,
+    "rotation": -119.99999999999999
+  },
+  "reversed": false,
+  "folder": "RT Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 68.19859051479783
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RT Corner 3 to OP.path b/src/deploy/pathplanner/paths/RT Corner 3 to OP.path
new file mode 100644
index 0000000..bdf838c
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RT Corner 3 to OP.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.4218690476190483,
+        "y": 0.63375
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.6718690476190483,
+        "y": 0.63375
+      },
+      "isLocked": false,
+      "linkedName": "RTCORNER3"
+    },
+    {
+      "anchor": {
+        "x": 0.290559523803116,
+        "y": 0.6445476190433237
+      },
+      "prevControl": {
+        "x": 0.5403016588556908,
+        "y": 0.6558995342729872
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "OP26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -6.33076493100981e-10
+  },
+  "reversed": false,
+  "folder": "RT Paths",
+  "idealStartingState": {
+    "velocity": 3.0,
+    "rotation": 150.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RT Corner3 to SUTO.path b/src/deploy/pathplanner/paths/RT Corner3 to SUTO.path
new file mode 100644
index 0000000..aa05f5d
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RT Corner3 to SUTO.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.2491071428625617,
+        "y": 0.7417261904740291
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.8603928571428576,
+        "y": 0.9900714285714287
+      },
+      "isLocked": false,
+      "linkedName": "RTCorner3"
+    },
+    {
+      "anchor": {
+        "x": 2.439285714285715,
+        "y": 4.026361904761904
+      },
+      "prevControl": {
+        "x": 2.4158050656367975,
+        "y": 3.777467029083384
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "SUTO26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 68.19859051479783
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RT Corner3-DP.path b/src/deploy/pathplanner/paths/RT Corner3-DP.path
new file mode 100644
index 0000000..51d0967
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RT Corner3-DP.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.4218690476190483,
+        "y": 0.63375
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.6717362513552696,
+        "y": 0.6418974227238853
+      },
+      "isLocked": false,
+      "linkedName": "RTCORNER3"
+    },
+    {
+      "anchor": {
+        "x": 0.9816071428571436,
+        "y": 5.902988095238095
+      },
+      "prevControl": {
+        "x": 1.4243095238095247,
+        "y": 6.064952380952381
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "DP26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "RT Paths",
+  "idealStartingState": {
+    "velocity": 5.2,
+    "rotation": 150.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RT to Climb.path b/src/deploy/pathplanner/paths/RT to Climb.path
new file mode 100644
index 0000000..6310dff
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RT to Climb.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 0.6445476190476187
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.8725938993821294,
+        "y": 0.5976932599443614
+      },
+      "isLocked": false,
+      "linkedName": "RT26"
+    },
+    {
+      "anchor": {
+        "x": 1.391916666672215,
+        "y": 3.7218690476211576
+      },
+      "prevControl": {
+        "x": 1.1917811386477197,
+        "y": 3.9394511022975047
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Climb26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "RT Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RT to DP.path b/src/deploy/pathplanner/paths/RT to DP.path
new file mode 100644
index 0000000..e7d8811
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RT to DP.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 0.6445476190476187
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.433972921667189,
+        "y": 1.064788231216431
+      },
+      "isLocked": false,
+      "linkedName": "RT26"
+    },
+    {
+      "anchor": {
+        "x": 0.9816071428571436,
+        "y": 5.902988095238095
+      },
+      "prevControl": {
+        "x": 1.0882648724172717,
+        "y": 5.676881640829826
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "DP26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "RT Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RT to OP.path b/src/deploy/pathplanner/paths/RT to OP.path
new file mode 100644
index 0000000..fe9ff54
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RT to OP.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 0.6445476190476187
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.067538824437326,
+        "y": 0.6495452543222722
+      },
+      "isLocked": false,
+      "linkedName": "RT26"
+    },
+    {
+      "anchor": {
+        "x": 0.290559523803116,
+        "y": 0.6445476190433237
+      },
+      "prevControl": {
+        "x": 1.4807136310339837,
+        "y": 0.6476139268426353
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "OP26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -6.33076493100981e-10
+  },
+  "reversed": false,
+  "folder": "RT Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RT to RTBump26.path b/src/deploy/pathplanner/paths/RT to RTBump26.path
new file mode 100644
index 0000000..704c404
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RT to RTBump26.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 0.6445476190476187
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.989964285714287,
+        "y": 1.098047619047619
+      },
+      "isLocked": false,
+      "linkedName": "RT26"
+    },
+    {
+      "anchor": {
+        "x": 3.605428571428572,
+        "y": 2.7932738095238094
+      },
+      "prevControl": {
+        "x": 3.184321428571429,
+        "y": 2.4909404761904774
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "RTBump"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 1.0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RT to Rotated BPBR.path b/src/deploy/pathplanner/paths/RT to Rotated BPBR.path
new file mode 100644
index 0000000..c3a4a72
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RT to Rotated BPBR.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 0.6445476190476187
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.825559523804199,
+        "y": 0.3962023809520122
+      },
+      "isLocked": false,
+      "linkedName": "RT26"
+    },
+    {
+      "anchor": {
+        "x": 8.291595238095239,
+        "y": 1.367988095238095
+      },
+      "prevControl": {
+        "x": 7.794904761904762,
+        "y": 0.6445476190476187
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Rotated BPBR"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 5.2,
+    "rotation": -80.0
+  },
+  "reversed": false,
+  "folder": "RT Paths",
+  "idealStartingState": {
+    "velocity": 5.0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RT to Shoot.path b/src/deploy/pathplanner/paths/RT to Shoot.path
new file mode 100644
index 0000000..7f06eb0
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RT to Shoot.path	
@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 0.6445476190476187
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.082964285714287,
+        "y": 0.5905595238095239
+      },
+      "isLocked": false,
+      "linkedName": "RT26"
+    },
+    {
+      "anchor": {
+        "x": 2.439285714285715,
+        "y": 4.026361904761904
+      },
+      "prevControl": {
+        "x": 2.538967242139329,
+        "y": 3.797094390698596
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "SUTO26"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5468879668049745,
+      "rotationDegrees": 32.66665927985974
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "RT Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RTBump - BPBR.path b/src/deploy/pathplanner/paths/RTBump - BPBR.path
new file mode 100644
index 0000000..86bf55e
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RTBump - BPBR.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.605428571428572,
+        "y": 2.7932738095238094
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.8425993959417326,
+        "y": 2.714216868021196
+      },
+      "isLocked": false,
+      "linkedName": "RTBump"
+    },
+    {
+      "anchor": {
+        "x": 8.291595238095239,
+        "y": 1.367988095238095
+      },
+      "prevControl": {
+        "x": 6.866309523806398,
+        "y": 1.4327738095281166
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Rotated BPBR"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 5.2,
+    "rotation": -80.0
+  },
+  "reversed": false,
+  "folder": "RT Paths",
+  "idealStartingState": {
+    "velocity": 3.0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RTBump to OP.path b/src/deploy/pathplanner/paths/RTBump to OP.path
new file mode 100644
index 0000000..c8e3872
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RTBump to OP.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.605428571428572,
+        "y": 2.7932738095238094
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.377238681524526,
+        "y": 2.0393310828547215
+      },
+      "isLocked": false,
+      "linkedName": "RTBump"
+    },
+    {
+      "anchor": {
+        "x": 0.290559523803116,
+        "y": 0.6445476190433237
+      },
+      "prevControl": {
+        "x": 0.9063910898366931,
+        "y": 1.0443555976694219
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "OP26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -6.33076493100981e-10
+  },
+  "reversed": false,
+  "folder": "RT Paths",
+  "idealStartingState": {
+    "velocity": 1.0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RTBump to RTCorner 3.path b/src/deploy/pathplanner/paths/RTBump to RTCorner 3.path
new file mode 100644
index 0000000..7beef85
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RTBump to RTCorner 3.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.605428571428572,
+        "y": 2.7932738095238094
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.580629070661129,
+        "y": 2.4961823219350183
+      },
+      "isLocked": false,
+      "linkedName": "RTBump"
+    },
+    {
+      "anchor": {
+        "x": 3.2491071428625617,
+        "y": 0.7417261904740291
+      },
+      "prevControl": {
+        "x": 3.386838705474557,
+        "y": 1.4099537101636623
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "RTCorner3"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 68.19859051479783
+  },
+  "reversed": false,
+  "folder": "RT Paths",
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RTBump to SUTO.path b/src/deploy/pathplanner/paths/RTBump to SUTO.path
new file mode 100644
index 0000000..0df9783
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RTBump to SUTO.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.605428571428572,
+        "y": 2.7932738095238094
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.7632142857142865,
+        "y": 2.523333333333334
+      },
+      "isLocked": false,
+      "linkedName": "RTBump"
+    },
+    {
+      "anchor": {
+        "x": 2.439285714285715,
+        "y": 4.026361904761904
+      },
+      "prevControl": {
+        "x": 2.3303954822596444,
+        "y": 3.801322091908026
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "SUTO26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.6,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "RT Paths",
+  "idealStartingState": {
+    "velocity": 2.6,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/Rotated BPBR to RT Corner 3.path b/src/deploy/pathplanner/paths/Rotated BPBR to RT Corner 3.path
new file mode 100644
index 0000000..541300f
--- /dev/null
+++ b/src/deploy/pathplanner/paths/Rotated BPBR to RT Corner 3.path	
@@ -0,0 +1,63 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.291595238095239,
+        "y": 1.367988095238095
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.603290598290597,
+        "y": 0.8953846153846157
+      },
+      "isLocked": false,
+      "linkedName": "Rotated BPBR"
+    },
+    {
+      "anchor": {
+        "x": 3.2491071428625617,
+        "y": 0.7417261904740291
+      },
+      "prevControl": {
+        "x": 5.870189676327198,
+        "y": 0.5271545527961041
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "RTCorner3"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.75,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 0.9,
+      "rotationDegrees": 14.999999999999998
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 68.19859051479783
+  },
+  "reversed": false,
+  "folder": "Return to Alliance",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -80.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/Rotated BPBR to RT.path b/src/deploy/pathplanner/paths/Rotated BPBR to RT.path
new file mode 100644
index 0000000..9d99f59
--- /dev/null
+++ b/src/deploy/pathplanner/paths/Rotated BPBR to RT.path	
@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.356380952380952,
+        "y": 1.4111785714285725
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.611345238092551,
+        "y": 0.579761904766525
+      },
+      "isLocked": false,
+      "linkedName": "BPBR26"
+    },
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 0.6445476190476187
+      },
+      "prevControl": {
+        "x": 3.9033044507423384,
+        "y": 0.6788314309971508
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "RT26"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.6763485477178409,
+      "rotationDegrees": -178.29621297113118
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 3.0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Return to Alliance",
+  "idealStartingState": {
+    "velocity": 5.2,
+    "rotation": 80.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/Rotated BPTL - LTBump.path b/src/deploy/pathplanner/paths/Rotated BPTL - LTBump.path
new file mode 100644
index 0000000..8e38837
--- /dev/null
+++ b/src/deploy/pathplanner/paths/Rotated BPTL - LTBump.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.291595238095239,
+        "y": 6.734404761908865
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.395068329523489,
+        "y": 6.938908219780141
+      },
+      "isLocked": false,
+      "linkedName": "Rotated BPTL"
+    },
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 5.222738095238095
+      },
+      "prevControl": {
+        "x": 3.8404217794989615,
+        "y": 5.112120113595268
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "LTBump26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 2.6,
+    "rotation": -0.0
+  },
+  "reversed": false,
+  "folder": "LT Paths",
+  "idealStartingState": {
+    "velocity": 5.2,
+    "rotation": 80.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/Rotated BPTL to LT Corrner 3.path b/src/deploy/pathplanner/paths/Rotated BPTL to LT Corrner 3.path
new file mode 100644
index 0000000..239e553
--- /dev/null
+++ b/src/deploy/pathplanner/paths/Rotated BPTL to LT Corrner 3.path	
@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.291595238095239,
+        "y": 6.734404761908865
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.8380952381004505,
+        "y": 7.155511904760722
+      },
+      "isLocked": false,
+      "linkedName": "Rotated BPTL"
+    },
+    {
+      "anchor": {
+        "x": 3.0655476190476194,
+        "y": 7.069130952380952
+      },
+      "prevControl": {
+        "x": 4.2247465424169555,
+        "y": 7.859472032875178
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "LTCorner3"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.7078891257995739,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 4.5,
+    "rotation": -71.565051177078
+  },
+  "reversed": false,
+  "folder": "Return to Alliance",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 80.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/Rotated BPTL to LT.path b/src/deploy/pathplanner/paths/Rotated BPTL to LT.path
new file mode 100644
index 0000000..684b8c5
--- /dev/null
+++ b/src/deploy/pathplanner/paths/Rotated BPTL to LT.path	
@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.291595238095239,
+        "y": 6.734404761908865
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.017476190476192,
+        "y": 7.403857142861247
+      },
+      "isLocked": false,
+      "linkedName": "Rotated BPTL"
+    },
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 7.382261904761905
+      },
+      "prevControl": {
+        "x": 3.972547619047618,
+        "y": 7.630607142857144
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "LT26"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5253112033195034,
+      "rotationDegrees": -6.537285236507412
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 5.0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Return to Alliance",
+  "idealStartingState": {
+    "velocity": 5.2,
+    "rotation": 80.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RotatedBPBR to RT Corner 3.path b/src/deploy/pathplanner/paths/RotatedBPBR to RT Corner 3.path
new file mode 100644
index 0000000..781b9fa
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RotatedBPBR to RT Corner 3.path	
@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.356380952380952,
+        "y": 1.4111785714285725
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.869657509157507,
+        "y": 1.0031762311762327
+      },
+      "isLocked": false,
+      "linkedName": "BPBR26"
+    },
+    {
+      "anchor": {
+        "x": 3.4218690476190483,
+        "y": 0.63375
+      },
+      "prevControl": {
+        "x": 5.894523809523811,
+        "y": 0.5041785714285723
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "RTCORNER3"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.7,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 5.2,
+    "rotation": 150.0
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 5.2,
+    "rotation": 80.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/RotatedBPTL to LT Corner 3.path b/src/deploy/pathplanner/paths/RotatedBPTL to LT Corner 3.path
new file mode 100644
index 0000000..0c4b422
--- /dev/null
+++ b/src/deploy/pathplanner/paths/RotatedBPTL to LT Corner 3.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.291595238095239,
+        "y": 6.734404761908865
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.788285714285713,
+        "y": 6.820785714285714
+      },
+      "isLocked": false,
+      "linkedName": "Rotated BPTL"
+    },
+    {
+      "anchor": {
+        "x": 3.0655476190476194,
+        "y": 7.069130952380952
+      },
+      "prevControl": {
+        "x": 4.19929761904762,
+        "y": 8.202880952380953
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "LTCorner3"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 5.2,
+    "rotation": -71.565051177078
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 5.2,
+    "rotation": 80.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/SUTO - OP.path b/src/deploy/pathplanner/paths/SUTO - OP.path
new file mode 100644
index 0000000..0c87749
--- /dev/null
+++ b/src/deploy/pathplanner/paths/SUTO - OP.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.439285714285715,
+        "y": 4.026361904761904
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.714240014354843,
+        "y": 4.267719672348951
+      },
+      "isLocked": false,
+      "linkedName": "SUTO26"
+    },
+    {
+      "anchor": {
+        "x": 0.290559523803116,
+        "y": 0.6445476190433237
+      },
+      "prevControl": {
+        "x": 0.41325459757958216,
+        "y": 0.8644620895863913
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "OP26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -6.33076493100981e-10
+  },
+  "reversed": false,
+  "folder": "SUTO to Places",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/SUTO to Climb.path b/src/deploy/pathplanner/paths/SUTO to Climb.path
new file mode 100644
index 0000000..e830a42
--- /dev/null
+++ b/src/deploy/pathplanner/paths/SUTO to Climb.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.439285714285715,
+        "y": 4.026361904761904
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.547261904761905,
+        "y": 3.5167142857142863
+      },
+      "isLocked": false,
+      "linkedName": "SUTO26"
+    },
+    {
+      "anchor": {
+        "x": 1.391916666672215,
+        "y": 3.7218690476211576
+      },
+      "prevControl": {
+        "x": 2.0181785714285723,
+        "y": 3.4627261904761912
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Climb26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "SUTO to Places",
+  "idealStartingState": {
+    "velocity": 2.0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/SUTO to DP.path b/src/deploy/pathplanner/paths/SUTO to DP.path
new file mode 100644
index 0000000..5859537
--- /dev/null
+++ b/src/deploy/pathplanner/paths/SUTO to DP.path	
@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.439285714285715,
+        "y": 4.026361904761904
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.6693272421120486,
+        "y": 3.928479855971913
+      },
+      "isLocked": false,
+      "linkedName": "SUTO26"
+    },
+    {
+      "anchor": {
+        "x": 0.9816071428571436,
+        "y": 5.902988095238095
+      },
+      "prevControl": {
+        "x": 1.1929475007770363,
+        "y": 5.534873778738688
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "DP26"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5103734439833985,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "SUTO to Places",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/SUTO to LT.path b/src/deploy/pathplanner/paths/SUTO to LT.path
new file mode 100644
index 0000000..1857a9e
--- /dev/null
+++ b/src/deploy/pathplanner/paths/SUTO to LT.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.439285714285715,
+        "y": 4.026361904761904
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.406892857142858,
+        "y": 6.108142857142857
+      },
+      "isLocked": false,
+      "linkedName": "SUTO26"
+    },
+    {
+      "anchor": {
+        "x": 3.6162261904761905,
+        "y": 7.382261904761905
+      },
+      "prevControl": {
+        "x": 2.2881190476190483,
+        "y": 7.468642857142858
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "LT26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 5.2,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "SUTO to Places",
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/SUTO to RT.path b/src/deploy/pathplanner/paths/SUTO to RT.path
new file mode 100644
index 0000000..59cd1f6
--- /dev/null
+++ b/src/deploy/pathplanner/paths/SUTO to RT.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.439285714285715,
+        "y": 4.026361904761904
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.452288922833184,
+        "y": 3.776700300650517
+      },
+      "isLocked": false,
+      "linkedName": "SUTO26"
+    },
+    {
+      "anchor": {
+        "x": 3.62702380952381,
+        "y": 0.6445476190476187
+      },
+      "prevControl": {
+        "x": 2.7200238095238105,
+        "y": 0.9360833333333336
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "RT26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Return to Alliance",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/Top to Bottom.path b/src/deploy/pathplanner/paths/Top to Bottom.path
new file mode 100644
index 0000000..317112c
--- /dev/null
+++ b/src/deploy/pathplanner/paths/Top to Bottom.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.356380952380952,
+        "y": 6.604833333333334
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.50754761904762,
+        "y": 5.233535714285715
+      },
+      "isLocked": false,
+      "linkedName": "BPTL26"
+    },
+    {
+      "anchor": {
+        "x": 8.356380952380952,
+        "y": 1.4111785714285725
+      },
+      "prevControl": {
+        "x": 8.518345238095238,
+        "y": 2.0050476190476196
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "BPBR26"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 5.2,
+    "rotation": 80.0
+  },
+  "reversed": false,
+  "folder": "Misc.",
+  "idealStartingState": {
+    "velocity": 5.2,
+    "rotation": 70.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/settings.json b/src/deploy/pathplanner/settings.json
new file mode 100644
index 0000000..c9aeeb6
--- /dev/null
+++ b/src/deploy/pathplanner/settings.json
@@ -0,0 +1,54 @@
+{
+  "robotWidth": 0.902,
+  "robotLength": 0.724,
+  "holonomicMode": true,
+  "pathFolders": [
+    "LT Paths",
+    "MS Paths",
+    "Misc.",
+    "RT Paths",
+    "Return to Alliance",
+    "SUTO to Places"
+  ],
+  "autoFolders": [
+    "ATW DualShot",
+    "Brendan Wants It",
+    "Bump Auto",
+    "ChudBot Autos",
+    "LT Auditorium Auto",
+    "LT Stem Lab Auto",
+    "Locked Autos",
+    "MS Auditorium Auto",
+    "MS Stem Lab Auto",
+    "PeoplePleaser",
+    "RT Auditorium Auto",
+    "RT Stem Lab Auto",
+    "Repetitive Auto",
+    "Useless Auto"
+  ],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "defaultNominalVoltage": 12.0,
+  "robotMass": 52.163,
+  "robotMOI": 6.883,
+  "robotTrackwidth": 0.546,
+  "driveWheelRadius": 0.051,
+  "driveGearing": 5.273,
+  "maxDriveSpeed": 5.45,
+  "driveMotorType": "krakenX60",
+  "driveCurrentLimit": 60.0,
+  "wheelCOF": 1.2,
+  "flModuleX": 0.238,
+  "flModuleY": 0.327,
+  "frModuleX": 0.238,
+  "frModuleY": -0.327,
+  "blModuleX": -0.238,
+  "blModuleY": 0.327,
+  "brModuleX": -0.238,
+  "brModuleY": -0.327,
+  "bumperOffsetX": 0.0,
+  "bumperOffsetY": 0.0,
+  "robotFeatures": []
+}
\ No newline at end of file

From 17362eb62c5a3003d36776ba4f78f67104c8d7be Mon Sep 17 00:00:00 2001
From: Maxwell Morgan 
Date: Thu, 19 Mar 2026 22:18:58 -0400
Subject: [PATCH 56/61] Test and update robot controls

---
 src/main/java/frc/robot/Constants.java        |   6 +-
 src/main/java/frc/robot/Robot.java            |   4 +
 src/main/java/frc/robot/RobotContainer.java   | 117 ++++++++++++------
 src/main/java/frc/robot/RobotState.java       |  27 ++--
 .../frc/robot/control/DefaultControls.java    |  17 ++-
 .../frc/robot/control/DriverController.java   | 114 +++++++++++++++++
 .../frc/robot/control/DriverControls.java     |  81 ++++++++----
 .../frc/robot/subsystems/drive/Drive.java     |   2 +
 .../robot/subsystems/guts/GutsIOTalonFX.java  |  18 +--
 .../frc/robot/subsystems/indexer/Indexer.java |   4 +-
 .../subsystems/indexer/IndexerIOTalonFX.java  |  10 +-
 .../frc/robot/subsystems/intake/Intake.java   |   3 +-
 .../subsystems/intake/IntakeConstants.java    |   8 +-
 .../subsystems/intake/IntakeIOTalonFX.java    |  18 ++-
 .../frc/robot/subsystems/shooter/Shooter.java |  14 ++-
 .../subsystems/shooter/ShooterConstants.java  |  23 ++--
 .../shooter/TrajectoryCalculator.java         |  16 +--
 .../subsystems/shooter/flywheel/Flywheel.java |   4 +
 .../shooter/flywheel/FlywheelIOTalonFX.java   |  10 +-
 .../robot/subsystems/shooter/hood/Hood.java   |  12 +-
 .../shooter/hood/HoodIOSparkMax.java          |   6 +-
 .../subsystems/shooter/turret/Turret.java     |  12 +-
 .../subsystems/shooter/turret/TurretIO.java   |   4 +-
 .../shooter/turret/TurretIOSparkMax.java      |  14 +--
 24 files changed, 388 insertions(+), 156 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index d867957..82465d7 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -81,9 +81,11 @@ public static final class DeviceIDs {
     public static final int kTurretHood = 13;
     public static final int kTurretAzimuth = 14;
 
-    public static final int kGuts = 15;
+    // Unused
+    public static final int kGuts = -1;
 
-    public static final int kIndexer = 16;
+    public static final int kIndexerTounge = 15;
+    public static final int kIndexerThroat = 16;
 
     public static final int kIntakeDrive = 17;
     public static final int kLeftIntakePivot = 18;
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 1e41966..c81e72c 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -7,6 +7,8 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.util.CachedSupplier;
@@ -70,6 +72,8 @@ public Robot() {
     Logger.start();
 
     robotContainer = new RobotContainer();
+    RobotState.getInstance().resetRotation(Rotation2d.kZero);
+    RobotState.getInstance().setPose(Pose2d.kZero);
   }
 
   /** This function is called periodically during all modes. */
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 54c37af..e3892ba 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -17,11 +17,28 @@
 import frc.robot.RobotState.OdometryObservation;
 import frc.robot.RobotState.VisionMeasurement;
 import frc.robot.control.Configurable;
+import frc.robot.control.DefaultControls;
 import frc.robot.control.DriverController;
-import frc.robot.control.ZoneControls;
+import frc.robot.control.DriverControls;
 import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.drive.DriveConstants.TunerConstants;
+import frc.robot.subsystems.drive.GyroIO;
 import frc.robot.subsystems.drive.GyroIOPigeon2;
-import frc.robot.subsystems.drive.ModuleIO;
+import frc.robot.subsystems.drive.ModuleIOSim;
+import frc.robot.subsystems.drive.ModuleIOTalonFX;
+import frc.robot.subsystems.indexer.Indexer;
+import frc.robot.subsystems.indexer.IndexerIOSim;
+import frc.robot.subsystems.indexer.IndexerIOTalonFX;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.intake.IntakeIOSim;
+import frc.robot.subsystems.intake.IntakeIOTalonFX;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.flywheel.FlywheelIOSim;
+import frc.robot.subsystems.shooter.flywheel.FlywheelIOTalonFX;
+import frc.robot.subsystems.shooter.hood.HoodIOSim;
+import frc.robot.subsystems.shooter.hood.HoodIOSparkMax;
+import frc.robot.subsystems.shooter.turret.TurretIOSim;
+import frc.robot.subsystems.shooter.turret.TurretIOSparkMax;
 import frc.robot.subsystems.vision.CameraIOLimelight;
 import frc.robot.subsystems.vision.Vision;
 import frc.robot.subsystems.vision.Vision.VisionConsumer;
@@ -29,15 +46,20 @@
 import java.util.List;
 import java.util.function.Supplier;
 
+import com.pathplanner.lib.auto.NamedCommands;
+
 public class RobotContainer {
   private final DriverController driver = new DriverController.XboxDriverController(0);
   private final DriverController operator = new DriverController.XboxDriverController(1);
 
-  private Vision vision;
   private Drive drive;
+  private Indexer indexer;
+  private Intake intake;
+  private Shooter shooter;
+  private Vision vision;
 
-  public static Field2d field2d = new Field2d();
-  public static Field2d targetField2d = new Field2d();
+  private static Field2d field2d = new Field2d();
+  private static Field2d targetField2d = new Field2d();
 
   public RobotContainer() {
 
@@ -47,43 +69,58 @@ public RobotContainer() {
     SmartDashboard.putData("TargetField", targetField2d);
     field2d.setRobotPose(RobotState.getInstance().getEstimatedPose());
     switch (Constants.kCurrentMode) {
-      case REAL:
-
-      case SIM:
-
-      case REPLAY:
-      default:
+      case REAL -> {
+        drive =
+            new Drive(
+                new GyroIOPigeon2(),
+                new ModuleIOTalonFX(TunerConstants.FrontLeft),
+                new ModuleIOTalonFX(TunerConstants.FrontRight),
+                new ModuleIOTalonFX(TunerConstants.BackLeft),
+                new ModuleIOTalonFX(TunerConstants.BackRight));
+        indexer = new Indexer(new IndexerIOTalonFX());
+        intake = new Intake(new IntakeIOTalonFX());
+        shooter =
+            new Shooter(new TurretIOSparkMax() {}, new HoodIOSparkMax(), new FlywheelIOTalonFX());
+        vision =
+            new Vision(
+                new VisionConsumer() {
+                  public void accept(
+                      Pose2d visionRobotPoseMeters,
+                      double timestampSeconds,
+                      edu.wpi.first.math.Matrix visionMeasurementStdDevs) {
+
+                    RobotState.getInstance()
+                        .addVisionMeasurement(
+                            new VisionMeasurement(
+                                timestampSeconds, visionRobotPoseMeters, visionMeasurementStdDevs));
+                  }
+                  ;
+                },
+                new CameraIOLimelight("limelight-front", robotRotationSupplier),
+                new CameraIOLimelight("limelight-one", robotRotationSupplier));
+      }
+      case SIM -> {
+        drive =
+            new Drive(
+                new GyroIO() {},
+                new ModuleIOSim(TunerConstants.FrontLeft),
+                new ModuleIOSim(TunerConstants.FrontRight),
+                new ModuleIOSim(TunerConstants.BackLeft),
+                new ModuleIOSim(TunerConstants.BackRight));
+        indexer = new Indexer(new IndexerIOSim());
+        intake = new Intake(new IntakeIOSim());
+        shooter = new Shooter(new TurretIOSim(), new HoodIOSim(), new FlywheelIOSim());
+      }
     }
-    vision =
-        new Vision(
-            new VisionConsumer() {
-              public void accept(
-                  Pose2d visionRobotPoseMeters,
-                  double timestampSeconds,
-                  edu.wpi.first.math.Matrix visionMeasurementStdDevs) {
-
-                RobotState.getInstance()
-                    .addVisionMeasurement(
-                        new VisionMeasurement(
-                            timestampSeconds, visionRobotPoseMeters, visionMeasurementStdDevs));
-              }
-              ;
-            },
-            new CameraIOLimelight("limelight-front", robotRotationSupplier),
-            new CameraIOLimelight("limelight-one", robotRotationSupplier));
-    drive =
-        new Drive(
-            new GyroIOPigeon2(),
-            new ModuleIO() {},
-            new ModuleIO() {},
-            new ModuleIO() {},
-            new ModuleIO() {});
 
     configureBindings();
   }
 
   private void configureBindings() {
-    List.of(new ZoneControls()).forEach(Configurable::configure);
+    List.of(
+            new DefaultControls(driver, operator, drive, indexer, intake, shooter),
+            new DriverControls(driver, operator, drive, shooter, intake, indexer))
+        .forEach(Configurable::configure);
   }
 
   public void robotPeriodic() {
@@ -100,11 +137,17 @@ public void robotPeriodic() {
                 drive.getRawGyroRotation()));
 
     targetField2d.setRobotPose(GeomUtil.toPose2d(RobotState.getInstance().getTurretTarget()));
+    field2d.setRobotPose(RobotState.getInstance().getEstimatedPose());
   }
 
   public Command getAutonomousCommand() {
     return Commands.print("No autonomous command configured");
   }
 
-  public void configurePathPlanner() {}
+  public void configurePathPlanner() {
+    NamedCommands.registerCommand("Shoot at Hub/Pass", shooter.shootAtTargetNoRotation(() -> RobotState.getInstance().getTurretTarget()));
+    NamedCommands.registerCommand("Index", indexer.index());
+    NamedCommands.registerCommand("Intake Deploy", intake.deployOpenLoop());
+    NamedCommands.registerCommand("Intake Retract", intake.retractOpenLoop());
+   }
 }
diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java
index 2e29a77..e098ee0 100644
--- a/src/main/java/frc/robot/RobotState.java
+++ b/src/main/java/frc/robot/RobotState.java
@@ -1,7 +1,5 @@
 package frc.robot;
 
-import static edu.wpi.first.units.Units.Meters;
-
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -29,8 +27,6 @@ public static RobotState getInstance() {
 
   private ChassisSpeeds robotVelocity = new ChassisSpeeds();
 
-  private Rotation2d gyroOffset = new Rotation2d();
-
   private RobotState() {
     poseEstimator =
         new SwerveDrivePoseEstimator(
@@ -96,11 +92,11 @@ public void setPose(
    * @param pose The pose to reset the pose estimator to.
    */
   public void setPose(Pose2d pose) {
-    poseEstimator.resetPosition(getRotation(), null, pose);
+    poseEstimator.resetPose(pose);
   }
 
   public void resetRotation(Rotation2d rotation) {
-    gyroOffset = poseEstimator.getEstimatedPosition().getRotation().minus(rotation);
+    poseEstimator.resetRotation(rotation);
   }
 
   /**
@@ -132,7 +128,7 @@ public ChassisSpeeds getRobotVelocity() {
 
   /** Get the rotation of the estimated pose. */
   public Rotation2d getRotation() {
-    return poseEstimator.getEstimatedPosition().getRotation().minus(gyroOffset);
+    return poseEstimator.getEstimatedPosition().getRotation();
   }
 
   public ChassisSpeeds getFieldVelocity() {
@@ -141,14 +137,15 @@ public ChassisSpeeds getFieldVelocity() {
 
   public Translation2d getTurretTarget() {
     Pose2d estimatedPose = getEstimatedPose();
-    if (estimatedPose.getX()
-        < AllianceFlipUtil.applyX(FieldConstants.LinesVertical.neutralZoneNear)) {
-      if (estimatedPose.getY() > AllianceFlipUtil.applyY(FieldConstants.LinesHorizontal.center)) {
-        return AllianceFlipUtil.apply(new Translation2d(Meters.of(2), Meters.of(1)));
-      }
-      return AllianceFlipUtil.apply(
-          new Translation2d(Meters.of(2), Meters.of(FieldConstants.fieldWidth - 1)));
-    }
+    // if (estimatedPose.getX()
+    //     < AllianceFlipUtil.applyX(FieldConstants.LinesVertical.neutralZoneNear)) {
+    //   if (estimatedPose.getY() > AllianceFlipUtil.applyY(FieldConstants.LinesHorizontal.center))
+    // {
+    //     return AllianceFlipUtil.apply(new Translation2d(Meters.of(2), Meters.of(1)));
+    //   }
+    //   return AllianceFlipUtil.apply(
+    //       new Translation2d(Meters.of(2), Meters.of(FieldConstants.fieldWidth - 1)));
+    // }
     return AllianceFlipUtil.apply(FieldConstants.Hub.innerCenterPoint.toTranslation2d());
   }
 
diff --git a/src/main/java/frc/robot/control/DefaultControls.java b/src/main/java/frc/robot/control/DefaultControls.java
index 98b5ec7..704521a 100644
--- a/src/main/java/frc/robot/control/DefaultControls.java
+++ b/src/main/java/frc/robot/control/DefaultControls.java
@@ -1,10 +1,17 @@
 package frc.robot.control;
 
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.Trajectory;
+import frc.robot.RobotState;
 import frc.robot.commands.DriveCommands;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.indexer.Indexer;
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.TrajectoryCalculator;
 
 public class DefaultControls implements Configurable {
 
@@ -31,14 +38,20 @@ public DefaultControls(
     this.shooter = shooter;
   }
 
-  /** Configure all default commands for the subsystems (e.g. includes joystick driving). */
+  /**
+   * Configure all default commands for the subsystems (e.g. includes joystick
+   * driving).
+   */
   @Override
   public void configure() {
     drive.setDefaultCommand(
         DriveCommands.joystickDrive(
             drive, () -> -driver.getLeftY(), () -> -driver.getLeftX(), () -> -driver.getRightX()));
 
+    Supplier targetPoseSupplier = () -> RobotState.getInstance().getTurretTarget();
     // Avoid the trench
-    shooter.setHoodDefaultCommand(shooter.hoodDown());
+    shooter.setHoodDefaultCommand(shooter.trackTargetHood(targetPoseSupplier));
+    shooter.setTurretDefaultCommand(
+        shooter.trackTargetTurret(targetPoseSupplier));
   }
 }
diff --git a/src/main/java/frc/robot/control/DriverController.java b/src/main/java/frc/robot/control/DriverController.java
index 25df11a..6a3d15e 100644
--- a/src/main/java/frc/robot/control/DriverController.java
+++ b/src/main/java/frc/robot/control/DriverController.java
@@ -1,6 +1,7 @@
 package frc.robot.control;
 
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller;
 import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -278,4 +279,117 @@ public void rumble(RumbleType rumbleType, double intensity) {
       controller.setRumble(rumbleType, intensity);
     }
   }
+
+  class PS4DriverController implements DriverController {
+    private final CommandPS4Controller controller;
+
+    public PS4DriverController(int controllerID) {
+      this.controller = new CommandPS4Controller(controllerID);
+    }
+
+    @Override
+    public Trigger aCross() {
+      return controller.cross();
+    }
+
+    @Override
+    public Trigger bCircle() {
+      return controller.circle();
+    }
+
+    @Override
+    public Trigger xSquare() {
+      return controller.square();
+    }
+
+    @Override
+    public Trigger yTriangle() {
+      return controller.triangle();
+    }
+
+    @Override
+    public Trigger leftBumper() {
+      return controller.L1();
+    }
+
+    @Override
+    public Trigger rightBumper() {
+      return controller.R1();
+    }
+
+    @Override
+    public Trigger leftTrigger() {
+      return controller.L2();
+    }
+
+    @Override
+    public Trigger rightTrigger() {
+      return controller.R2();
+    }
+
+    @Override
+    public Trigger dPadUp() {
+      return controller.povUp();
+    }
+
+    @Override
+    public Trigger dPadUpLeft() {
+      return controller.povUpLeft();
+    }
+
+    @Override
+    public Trigger dPadUpRight() {
+      return controller.povUpRight();
+    }
+
+    @Override
+    public Trigger dPadDown() {
+      return controller.povDown();
+    }
+
+    @Override
+    public Trigger dPadDownLeft() {
+      return controller.povDownLeft();
+    }
+
+    @Override
+    public Trigger dPadDownRight() {
+      return controller.povDownRight();
+    }
+
+    @Override
+    public Trigger dPadLeft() {
+      return controller.povLeft();
+    }
+
+    @Override
+    public Trigger dPadRight() {
+      return controller.povRight();
+    }
+
+    @Override
+    public double getLeftX() {
+      return controller.getLeftX();
+    }
+
+    @Override
+    public double getLeftY() {
+      return controller.getLeftY();
+    }
+
+    @Override
+    public double getRightX() {
+      return controller.getRightX();
+    }
+
+    @Override
+    public double getRightY() {
+      return controller.getRightY();
+    }
+
+    @Override
+    public void rumble(RumbleType rumbleType, double intensity) {
+      controller.setRumble(rumbleType, intensity);
+    }
+  }
 }
diff --git a/src/main/java/frc/robot/control/DriverControls.java b/src/main/java/frc/robot/control/DriverControls.java
index 4229f22..d4bd8b4 100644
--- a/src/main/java/frc/robot/control/DriverControls.java
+++ b/src/main/java/frc/robot/control/DriverControls.java
@@ -1,10 +1,11 @@
 package frc.robot.control;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.StartEndCommand;
+import frc.robot.RobotState;
 import frc.robot.commands.DriveCommands;
 import frc.robot.subsystems.drive.Drive;
-import frc.robot.subsystems.guts.Guts;
 import frc.robot.subsystems.indexer.Indexer;
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.shooter.Shooter;
@@ -15,7 +16,6 @@ public class DriverControls implements Configurable {
   private final DriverController operator;
   private final Drive drive;
   private final Shooter shooter;
-  private final Guts guts;
   private final Intake intake;
   private final Indexer indexer;
 
@@ -24,26 +24,27 @@ public DriverControls(
       DriverController operator,
       Drive drive,
       Shooter shooter,
-      Guts guts,
       Intake intake,
       Indexer indexer) {
     this.driver = driver;
     this.operator = operator;
     this.drive = drive;
     this.shooter = shooter;
-    this.guts = guts;
     this.intake = intake;
     this.indexer = indexer;
   }
 
   @Override
   public void configure() {
-    configureDriverControls();
-    configureOperatorControls();
+    configureSingleController();
   }
 
   private void configureDriverControls() {
-    driver.xSquare().onTrue(Commands.runOnce(drive::zeroYaw, drive));
+    driver
+        .xSquare()
+        .onTrue(
+            Commands.runOnce(
+                () -> RobotState.getInstance().resetRotation(Rotation2d.kZero), drive));
     driver.bCircle().onTrue(Commands.runOnce(drive::stopWithX, drive));
 
     driver.dPadUp().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTH));
@@ -76,15 +77,9 @@ private void configureDriverControls() {
   private void configureOperatorControls() {
     operator.leftBumper().and(operator.leftTrigger().negate()).whileTrue(intake.intake());
 
-    operator
-        .rightBumper()
-        .whileTrue(
-            shooter
-                .setFlywheelVelocity(8500));
+    operator.rightBumper().whileTrue(shooter.setFlywheelVelocity(8500));
 
-    operator
-        .rightTrigger()
-        .whileTrue(guts.runGutForward());
+    operator.rightTrigger().whileTrue(indexer.index());
 
     operator
         .dPadUp()
@@ -112,12 +107,54 @@ private void configureOperatorControls() {
     operator.aCross().whileTrue(intake.outtake());
     operator.xSquare().whileTrue(intake.deployOpenLoop());
     operator.yTriangle().whileTrue(intake.retractOpenLoop());
-    operator
-        .bCircle()
-        .whileTrue(
-            shooter
-                .setFlywheelVelocity(2000)
-                .alongWith(
-                    guts.runGutForward()));
+    operator.bCircle().whileTrue(shooter.setFlywheelVelocity(2000).alongWith(indexer.index()));
+
+    // operator.aCross().whileTrue(shooter.shootAtTargetNoRotation(() ->
+    // RobotState.getInstance().getTurretTarget()));
+    // operator.aCross().and(shooter::readyToShoot).whileTrue(indexer.index());
+  }
+
+  private void configureSingleController() {
+
+    driver.rightBumper().whileTrue(shooter.setFlywheelVelocity(3000));
+    // // RB -> Shoot
+    // driver
+    // .rightBumper()
+    // .whileTrue(
+    // Commands.runEnd(
+    // () -> shooter.setFlywheelOpenLoop(.0175),
+    // () -> shooter.setFlywheelOpenLoop(0),
+    // shooter));
+    // driver
+    // .leftBumper()
+    // .whileTrue(
+    // Commands.runEnd(
+    // () -> shooter.setFlywheelOpenLoop(.0185),
+    // () -> shooter.setFlywheelOpenLoop(0),
+    // shooter));
+
+    driver.aCross().whileTrue(indexer.index());
+    driver.bCircle().whileTrue(indexer.indexReverse());
+
+    // driver
+    // .aCross()
+    // .whileTrue(
+    // Commands.runEnd(
+    // () -> indexer.setThroatOpenLoop(0.5), () -> indexer.setThroatOpenLoop(0),
+    // indexer));
+    // // driver
+    // // .bCircle()
+    // // .whileTrue(
+    // // Commands.runEnd(
+    // // () -> indexer.setThroatOpenLoop(-0.5),
+    // // () -> indexer.setThroatOpenLoop(0),
+    // // indexer));
+
+    driver.xSquare().whileTrue(intake.retractOpenLoop());
+    driver.yTriangle().whileTrue(intake.deployOpenLoop());
+
+    driver.leftTrigger().whileTrue(intake.outtake());
+    driver.rightTrigger().whileTrue(intake.intake());
+    
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
index 264eb2a..58dcebe 100644
--- a/src/main/java/frc/robot/subsystems/drive/Drive.java
+++ b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -81,6 +81,8 @@ public Drive(
                 (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())),
             new SysIdRoutine.Mechanism(
                 (voltage) -> runCharacterization(voltage.in(Volts)), null, this));
+
+    zeroYaw();
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/guts/GutsIOTalonFX.java b/src/main/java/frc/robot/subsystems/guts/GutsIOTalonFX.java
index a7c1e72..0f47a4e 100644
--- a/src/main/java/frc/robot/subsystems/guts/GutsIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/guts/GutsIOTalonFX.java
@@ -6,13 +6,6 @@
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.hardware.TalonFX;
-import com.revrobotics.PersistMode;
-import com.revrobotics.RelativeEncoder;
-import com.revrobotics.ResetMode;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.config.SparkMaxConfig;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.units.measure.Voltage;
@@ -28,7 +21,7 @@
 public class GutsIOTalonFX implements GutsIO {
 
   private final TalonFX motor = new TalonFX(DeviceIDs.kGuts);
-  
+
   private final StatusSignal velocitySignal;
   private final StatusSignal voltageSignal;
   private final StatusSignal currentSignal;
@@ -39,7 +32,7 @@ public GutsIOTalonFX() {
     velocitySignal = motor.getVelocity();
     voltageSignal = motor.getMotorVoltage();
     currentSignal = motor.getSupplyCurrent();
-    
+
     tryUntilOk(5, () -> motor.getConfigurator().apply(motorConfig));
 
     BaseStatusSignal.setUpdateFrequencyForAll(50, velocitySignal, voltageSignal, currentSignal);
@@ -47,11 +40,8 @@ public GutsIOTalonFX() {
   }
 
   @Override
-  public void setOpenLoop(double speed) {
-  }
+  public void setOpenLoop(double speed) {}
 
   @Override
-  public void updateInputs(GutsIOInputs inputs) {
-
-  }
+  public void updateInputs(GutsIOInputs inputs) {}
 }
diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java
index ddec03d..4028103 100644
--- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java
+++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java
@@ -28,7 +28,7 @@ public Command index() {
     return Commands.startEnd(
         () -> {
           io.setThroatOpenLoop(IndexerConstants.kThroatMotorSpeed);
-          io.setToungeOpenLoop(IndexerConstants.kToungeMotorSpeed);
+          io.setToungeOpenLoop(-IndexerConstants.kThroatMotorSpeed);
         },
         () -> {
           io.stop();
@@ -40,7 +40,7 @@ public Command indexReverse() {
     return Commands.startEnd(
         () -> {
           io.setThroatOpenLoop(-IndexerConstants.kThroatMotorSpeed);
-          io.setToungeOpenLoop(-IndexerConstants.kToungeMotorSpeed);
+          io.setToungeOpenLoop(IndexerConstants.kThroatMotorSpeed);
         },
         () -> {
           io.stop();
diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java
index 84b71bb..7db0cdc 100644
--- a/src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java
@@ -1,10 +1,12 @@
 package frc.robot.subsystems.indexer;
 
+import static edu.wpi.first.units.Units.Amps;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static frc.robot.util.PhoenixUtil.tryUntilOk;
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.hardware.ParentDevice;
 import com.ctre.phoenix6.hardware.TalonFX;
@@ -15,8 +17,8 @@
 import frc.robot.Constants.DeviceIDs;
 
 public class IndexerIOTalonFX implements IndexerIO {
-  private final TalonFX throatMotor = new TalonFX(DeviceIDs.kIndexer);
-  private final TalonFX toungeMotor = new TalonFX(DeviceIDs.kIndexer);
+  private final TalonFX throatMotor = new TalonFX(DeviceIDs.kIndexerThroat);
+  private final TalonFX toungeMotor = new TalonFX(DeviceIDs.kIndexerTounge);
 
   private final StatusSignal throatVelocity;
   private final StatusSignal throatVoltage;
@@ -27,7 +29,9 @@ public class IndexerIOTalonFX implements IndexerIO {
   private final StatusSignal toungeCurrent;
 
   public IndexerIOTalonFX() {
-    TalonFXConfiguration throatMotorConfig = new TalonFXConfiguration();
+    TalonFXConfiguration throatMotorConfig =
+        new TalonFXConfiguration()
+            .withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(Amps.of(30)));
     TalonFXConfiguration toungeMotorConfig = new TalonFXConfiguration();
 
     throatVelocity = throatMotor.getVelocity();
diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
index 1d47de9..2228ffb 100644
--- a/src/main/java/frc/robot/subsystems/intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -44,7 +44,8 @@ public Command retractOpenLoop() {
   }
 
   public Command deployPosition() {
-    return Commands.run(() -> io.setPivotPosition(IntakeConstants.kExtensionPositionRotations), this);
+    return Commands.run(
+        () -> io.setPivotPosition(IntakeConstants.kExtensionPositionRotations), this);
   }
 
   public Command retractPosition() {
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
index fe80a7f..e15a6c5 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -3,15 +3,15 @@
 import com.ctre.phoenix6.configs.Slot0Configs;
 
 public final class IntakeConstants {
-  public static final double kPivotMotorSpeed = 0.4;
-  public static final double kRollerMotorSpeed = -0.8;
+  public static final double kPivotMotorSpeed = 0.1;
+  public static final double kRollerMotorSpeed = 0.8;
   public static final double kSignificantlyFasterRollerMotorSpeed = -0.75;
 
   // TODO: Tune
   public static final double kExtensionPositionRotations = 100.0;
 
-  // Change Gear Ratios later
-  public static final double kPivotMotorGearRatio = 1.0;
+  public static final double kLeftPivotMotorGearRatio = 43.0 / 14.0;
+  public static final double kRightPivotMotorGearRatio = 45.0 / 14.0;
   public static final double kRollerMotorGearRatio = 1.0;
 
   public static final Slot0Configs kPivotGains =
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java
index 83a4ecc..9097031 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java
@@ -4,10 +4,12 @@
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.MotorAlignmentValue;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
@@ -16,7 +18,7 @@
 
 public class IntakeIOTalonFX implements IntakeIO {
   private TalonFX leftPivotMotor = new TalonFX(DeviceIDs.kLeftIntakePivot);
-  private TalonFX rightPivotMotor = new TalonFX(DeviceIDs.kLeftIntakePivot);
+  private TalonFX rightPivotMotor = new TalonFX(DeviceIDs.kRightIntakePivot);
   private TalonFX driveMotor = new TalonFX(DeviceIDs.kIntakeDrive);
 
   private Follower rightPivotFollower =
@@ -42,16 +44,19 @@ public class IntakeIOTalonFX implements IntakeIO {
 
   public IntakeIOTalonFX() {
     leftPivotConfig = new TalonFXConfiguration().withSlot0(IntakeConstants.kPivotGains);
-    rightPivotConfig = new TalonFXConfiguration().withSlot0(IntakeConstants.kPivotGains);
+    rightPivotConfig =
+        new TalonFXConfiguration()
+            .withSlot0(IntakeConstants.kPivotGains)
+            .withMotorOutput(
+                new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive));
+    driveMotorConfig = new TalonFXConfiguration();
 
     leftPivotMotor.setPosition(0);
     rightPivotMotor.setPosition(0);
 
     leftPivotMotor.getConfigurator().apply(leftPivotConfig);
     rightPivotMotor.getConfigurator().apply(rightPivotConfig);
-    driveMotor.getConfigurator().apply(driveMotorConfig);
-
-    rightPivotMotor.setControl(rightPivotFollower);
+    // driveMotor.getConfigurator().apply(driveMotorConfig);
 
     leftPivotVelocity = leftPivotMotor.getVelocity();
     leftPivotVoltage = leftPivotMotor.getMotorVoltage();
@@ -103,12 +108,13 @@ public void updateInputs(IntakeIOInputs inputs) {
   @Override
   public void setPivotPosition(double positionRotations) {
     leftPivotMotor.setControl(positionRequest.withPosition(positionRotations));
+    rightPivotMotor.setControl(positionRequest.withPosition(positionRotations));
   }
 
   @Override
   public void setPivotSpeed(double speed) {
     leftPivotMotor.set(speed);
-    rightPivotMotor.setControl(rightPivotFollower);
+    rightPivotMotor.set(speed * -0.95);
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index a36b17f..c3a90ed 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -4,6 +4,7 @@
 
 package frc.robot.subsystems.shooter;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -39,9 +40,12 @@ public void periodic() {
     flywheel.periodic();
   }
 
+  public boolean readyToShoot() {
+    return turret.atGoal() && hood.atGoal() && flywheel.atGoal();
+  }
+
   /**
-   * Apply a pre-calculated shooter command to this shooter. This does not require
-   * the shooter
+   * Apply a pre-calculated shooter command to this shooter. This does not require the shooter
    * subsystem - use when combining with other shooters.
    *
    * @param cmd The shot parameters to apply.
@@ -89,7 +93,11 @@ public Command hoodDown() {
     return hood.down();
   }
 
-  public Command trackTarget(Supplier targetSupplier) {
+  public Command trackTargetHood(Supplier targetSupplier) {
+    return hood.trackTarget(targetSupplier);
+  }
+
+  public Command trackTargetTurret(Supplier targetSupplier) {
     return turret.trackTarget(targetSupplier);
   }
 
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index 9163893..fac06ac 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -16,21 +16,22 @@ public final class ShooterConstants {
   public static final double kLatencySeconds = 0.05;
 
   public static final class TurretConstants {
-    public static final double kGearRatio = 200 / 19; // Motor / Turret
-    public static final double kMinTurretAngleRad = Units.degreesToRadians(-120);
-    public static final double kMaxTurretAngleRad = Units.degreesToRadians(120);
+    public static final double kGearRatio = 200.0 / 20.0; // Motor / Turret
+    public static final double kMinTurretAngleRad = Units.degreesToRadians(-90);
+    public static final double kMaxTurretAngleRad = Units.degreesToRadians(210);
     public static final double kAngleTolerance = Units.degreesToRadians(0.5);
 
+    //
     // +X = Forward, +Y = Left
     public static final Transform3d kRobotToTurret =
-        new Transform3d(Inches.of(3.749), Inches.of(8.186), Inches.of(13.401), Rotation3d.kZero);
+        new Transform3d(Inches.of(7.5), Inches.of(-4), Inches.of(14.5), Rotation3d.kZero);
   }
 
   public static final class HoodConstants {
     public static final double kTurretToHoodInches = 1.878;
     public static final double kGearRatio = 16 / 1;
 
-    public static final double kAngleTolerance = Units.degreesToRadians(5);
+    public static final double kAngleTolerance = Units.degreesToRadians(1);
 
     public static final Transform3d kRobotToHood =
         new Transform3d(
@@ -46,7 +47,7 @@ public static final class HoodConstants {
 
     public static final double kMinAngleRad = Units.degreesToRadians(0);
     // TODO: Tune
-    public static final double kMaxAngleRad = 5.9;
+    public static final double kMaxAngleRad = 3.9;
   }
 
   public static final class FlywheelConstants {
@@ -55,11 +56,11 @@ public static final class FlywheelConstants {
 
     public static final Slot0Configs kGains =
         new Slot0Configs()
-            .withKP(0.75)
-            .withKI(0)
-            .withKD(0.0)
-            .withKS(0.0225 * 12)
-            .withKV(0.0945)
+            .withKP(0.1)
+            .withKI(0.1)
+            .withKD(0.0025)
+            .withKS(0.17 * 12)
+            .withKV(0.042)
             .withKA(0);
     public static final MotorOutputConfigs kOutputConfigs =
         new MotorOutputConfigs()
diff --git a/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java
index aad941f..047032a 100644
--- a/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java
+++ b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java
@@ -23,15 +23,11 @@ public class TrajectoryCalculator {
   private static final double MAX_SHOOTING_DISTANCE = 5.0;
 
   static {
-    shooterTable.put(1.5, new TrajectoryParams(2800.0, 0, 0.38));
-    shooterTable.put(2.0, new TrajectoryParams(3100.0, 0.0349, 0.45));
-    shooterTable.put(2.5, new TrajectoryParams(3250.0, 0.0698, 0.52));
-    shooterTable.put(3.0, new TrajectoryParams(3650.0, 0.104, 0.60));
-    shooterTable.put(3.5, new TrajectoryParams(3900.0, 0.1396, 0.68));
-    shooterTable.put(4.0, new TrajectoryParams(4100.0, 0.174, 0.76));
-    shooterTable.put(4.5, new TrajectoryParams(4350.0, 0.209, 0.85));
-    shooterTable.put(5.0, new TrajectoryParams(4550.0, 0.244, 0.94));
-    shooterTable.put(5.5, new TrajectoryParams(4550.0, 0.279, 1.05));
+    shooterTable.put(2.36, new TrajectoryParams(2000.0, 0, 0.45));
+    shooterTable.put(2.6, new TrajectoryParams(2250.0, -0.5, 0.52));
+    shooterTable.put(3.0, new TrajectoryParams(2500.0, -1.0, 0.60));
+    shooterTable.put(3.5, new TrajectoryParams(2600.0, -3.0, 0.68));
+    shooterTable.put(4.0, new TrajectoryParams(3000.0, -3.9, 0.76));
   }
 
   // ========== PUBLIC API ==========
@@ -50,7 +46,7 @@ public static double calculateRPM(Translation2d targetLocation, Pose2d robotPose
   }
 
   public static double calculateHoodAngle(Translation2d targetLocation, Pose2d robotPose) {
-    return 20 * shooterTable.get(targetLocation.getDistance(robotPose.getTranslation())).hoodAngle;
+    return shooterTable.get(targetLocation.getDistance(robotPose.getTranslation())).hoodAngle;
   }
 
   // ========== PRIVATE IMPLEMENTATION ==========
diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java
index 4ebfe75..776cbab 100644
--- a/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java
+++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java
@@ -77,4 +77,8 @@ public void stop() {
   public double getVelocity() {
     return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec);
   }
+
+  public boolean atGoal() {
+    return atGoal;
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java
index e7fe921..9f32a22 100644
--- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java
@@ -31,12 +31,10 @@ public FlywheelIOTalonFX() {
     motorConfig =
         new TalonFXConfiguration()
             .withSlot0(FlywheelConstants.kGains)
-            /**
-             * TODO: Update gains Peiwei, Ben: see the FlywheelConstants.kGains above... thats where
-             * the values are You also might have to check if the inverted values are correct,
-             * positive should spin the right way for shooting (line above that has the
-             * withInverted() method)
-             */
+            // .withCurrentLimits(
+            //     new CurrentLimitsConfigs()
+            //         .withSupplyCurrentLimit(Amps.of(50))
+            //         .withStatorCurrentLimit(Amps.of(50)))
             .withMotorOutput(FlywheelConstants.kOutputConfigs);
     tryUntilOk(5, () -> motor.getConfigurator().apply(motorConfig, 0.25));
 
diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java b/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java
index 49952c1..edacb5d 100644
--- a/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java
+++ b/src/main/java/frc/robot/subsystems/shooter/hood/Hood.java
@@ -23,6 +23,7 @@ public class Hood extends SubsystemBase {
   private final HoodIOInputsAutoLogged inputs = new HoodIOInputsAutoLogged();
 
   private double targetAngleRad = 0.0;
+  private boolean closedLoop = false;
 
   private boolean atGoal = false;
   private Debouncer atGoalDebouncer = new Debouncer(0.2, DebounceType.kFalling);
@@ -39,7 +40,9 @@ public void periodic() {
 
     RobotVisualizer.getInstance().setTurretHoodAngle(inputs.positionRad);
 
-    io.setAngle(targetAngleRad);
+    if (closedLoop) {
+      io.setAngle(targetAngleRad);
+    }
   }
 
   public Command trackTarget(Supplier targetSupplier) {
@@ -65,12 +68,15 @@ public Command down() {
    * @param angle The target angle (in radians).
    */
   public void setAngle(double angle) {
-    atGoal = atGoalDebouncer.calculate(
-        Math.abs(angle - inputs.positionRad) < HoodConstants.kAngleTolerance);
+    closedLoop = true;
+    atGoal =
+        atGoalDebouncer.calculate(
+            Math.abs(angle - inputs.positionRad) < HoodConstants.kAngleTolerance);
     targetAngleRad = angle;
   }
 
   public void setOpenLoop(double output) {
+    closedLoop = false;
     io.setOpenLoop(output);
   }
 
diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
index e8ec6c6..8347fb9 100644
--- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
@@ -7,6 +7,7 @@
 import com.revrobotics.PersistMode;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.ResetMode;
+import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
@@ -40,8 +41,11 @@ public HoodIOSparkMax() {
         .positionConversionFactor(2 * Math.PI / HoodConstants.kGearRatio) // No absolute encoder...
         .velocityConversionFactor(2 * Math.PI / HoodConstants.kGearRatio / 60.0);
 
+    // TODO: Tune
     config.closedLoop.feedForward.kS(0.015 * 12);
-    config.closedLoop.p(0.1);
+    config.closedLoop.p(0.5);
+
+    config.closedLoop.allowedClosedLoopError(HoodConstants.kAngleTolerance, ClosedLoopSlot.kSlot0);
 
     tryUntilOk(
         motor,
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
index 447e29b..2182781 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
@@ -12,7 +12,6 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import frc.robot.RobotContainer;
 import frc.robot.RobotState;
 import frc.robot.RobotVisualizer;
 import frc.robot.subsystems.shooter.ShooterConstants.TurretConstants;
@@ -62,9 +61,9 @@ public Command trackTarget(Supplier targetSupplier) {
         () -> {
           Translation2d target = targetSupplier.get();
           Pose2d robotPose = RobotState.getInstance().getEstimatedPose();
-          RobotContainer.field2d.setRobotPose(robotPose);
 
-          Translation2d turretOffset = Translation2d.kZero;
+          Translation2d turretOffset =
+              TurretConstants.kRobotToTurret.getTranslation().toTranslation2d();
 
           // Turret position in field coordinates
           Translation2d turretFieldPos =
@@ -111,7 +110,12 @@ public void setPosition(Rotation2d position) {
 
   public void setOpenLoop(double output) {
     outputs.mode = TurretIOOutputMode.OPEN_LOOP;
-    outputs.openLoopOutput = MathUtil.clamp(output, -1.0, 1.0);
+    if (inputs.positionRad > TurretConstants.kMaxTurretAngleRad
+        || inputs.positionRad < TurretConstants.kMinTurretAngleRad) {
+      outputs.openLoopOutput = 0.0;
+    } else {
+      outputs.openLoopOutput = MathUtil.clamp(output, -1.0, 1.0);
+    }
   }
 
   public void stop() {
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java
index b126647..6fad939 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java
@@ -26,7 +26,7 @@ public class TurretIOOutputs {
     public Rotation2d closedLoopTarget = Rotation2d.kZero;
   }
 
-  void updateInputs(TurretIOInputs inputs);
+  default void updateInputs(TurretIOInputs inputs) {}
 
-  void applyOutputs(TurretIOOutputs outputs);
+  default void applyOutputs(TurretIOOutputs outputs) {}
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
index 9d2cb8d..bc58698 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
@@ -4,7 +4,6 @@
 import static frc.robot.util.SparkUtil.sparkStickyFault;
 import static frc.robot.util.SparkUtil.tryUntilOk;
 
-import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.PersistMode;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.ResetMode;
@@ -25,17 +24,14 @@
 
 public class TurretIOSparkMax implements TurretIO {
   private final SparkMax motor;
-  private final AbsoluteEncoder encoder;
+  private final RelativeEncoder encoder;
   private final SparkClosedLoopController motorController;
 
   private final Debouncer connectedDebouncer = new Debouncer(0.5, DebounceType.kFalling);
 
   public TurretIOSparkMax() {
-    motor =
-        new SparkMax(
-            DeviceIDs.kTurretAzimuth,
-            MotorType.kBrushless);
-    encoder = motor.getAbsoluteEncoder();
+    motor = new SparkMax(DeviceIDs.kTurretAzimuth, MotorType.kBrushless);
+    encoder = motor.getEncoder();
     motorController = motor.getClosedLoopController();
 
     SparkMaxConfig config = new SparkMaxConfig();
@@ -48,6 +44,8 @@ public TurretIOSparkMax() {
             2 * Math.PI / TurretConstants.kGearRatio) // No absolute encoder...
         .velocityConversionFactor(2 * Math.PI / TurretConstants.kGearRatio / 60.0);
 
+    // config.absoluteEncoder.apply(new
+    // AbsoluteEncoderConfig().zeroOffset(0.495).zeroCentered(true));
     config.closedLoop.positionWrappingEnabled(true).feedbackSensor(FeedbackSensor.kPrimaryEncoder);
 
     config.softLimit.reverseSoftLimitEnabled(false).forwardSoftLimitEnabled(false);
@@ -65,7 +63,7 @@ public TurretIOSparkMax() {
         () ->
             motor.configure(
                 config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters));
-    tryUntilOk(motor, 5, () -> motor.getEncoder().setPosition(encoder.getPosition()));
+    encoder.setPosition(0);
   }
 
   @Override

From 057dc1f4a573deddf6cf419b82b413e75082c481 Mon Sep 17 00:00:00 2001
From: Legion 
Date: Thu, 19 Mar 2026 23:26:18 -0400
Subject: [PATCH 57/61] Update autos (please work)

---
 src/deploy/pathplanner/autos/URI Center.auto  | 70 ++++++++++++++
 .../pathplanner/autos/URI Left Depot.auto     | 95 +++++++++++++++++++
 .../pathplanner/autos/URI Right Outpost.auto  | 88 +++++++++++++++++
 src/deploy/pathplanner/navgrid.json           |  2 +-
 .../pathplanner/paths/C to C Tower.path       | 54 +++++++++++
 .../paths/Depot to Left Tower.path            | 54 +++++++++++
 .../pathplanner/paths/L Trench to Depot.path  | 54 +++++++++++
 .../paths/Outpost Int Ext to Right Tower.path | 54 +++++++++++
 .../paths/R Trench to Depot Intake Ext.path   | 54 +++++++++++
 src/deploy/pathplanner/settings.json          | 29 ++----
 src/main/deploy/pathplanner/navgrid.json      |  1 +
 11 files changed, 531 insertions(+), 24 deletions(-)
 create mode 100644 src/deploy/pathplanner/autos/URI Center.auto
 create mode 100644 src/deploy/pathplanner/autos/URI Left Depot.auto
 create mode 100644 src/deploy/pathplanner/autos/URI Right Outpost.auto
 create mode 100644 src/deploy/pathplanner/paths/C to C Tower.path
 create mode 100644 src/deploy/pathplanner/paths/Depot to Left Tower.path
 create mode 100644 src/deploy/pathplanner/paths/L Trench to Depot.path
 create mode 100644 src/deploy/pathplanner/paths/Outpost Int Ext to Right Tower.path
 create mode 100644 src/deploy/pathplanner/paths/R Trench to Depot Intake Ext.path
 create mode 100644 src/main/deploy/pathplanner/navgrid.json

diff --git a/src/deploy/pathplanner/autos/URI Center.auto b/src/deploy/pathplanner/autos/URI Center.auto
new file mode 100644
index 0000000..8a59019
--- /dev/null
+++ b/src/deploy/pathplanner/autos/URI Center.auto	
@@ -0,0 +1,70 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "C to C Tower"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 8.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Shoot"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.0
+                      }
+                    },
+                    {
+                      "type": "deadline",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "wait",
+                            "data": {
+                              "waitTime": 7.0
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Index"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "URI Autos",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/URI Left Depot.auto b/src/deploy/pathplanner/autos/URI Left Depot.auto
new file mode 100644
index 0000000..c98c827
--- /dev/null
+++ b/src/deploy/pathplanner/autos/URI Left Depot.auto	
@@ -0,0 +1,95 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "DeployIntake"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "L Trench to Depot"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Depot to Left Tower"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 8.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Shoot"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.0
+                      }
+                    },
+                    {
+                      "type": "deadline",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "wait",
+                            "data": {
+                              "waitTime": 7.0
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Index"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "URI Autos",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/URI Right Outpost.auto b/src/deploy/pathplanner/autos/URI Right Outpost.auto
new file mode 100644
index 0000000..076e754
--- /dev/null
+++ b/src/deploy/pathplanner/autos/URI Right Outpost.auto	
@@ -0,0 +1,88 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "DeployIntake"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "R Trench to Depot Intake Ext"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 5.0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Outpost Int Ext to Right Tower"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 8.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Shoot"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.0
+                      }
+                    },
+                    {
+                      "type": "deadline",
+                      "data": {
+                        "commands": [
+                          {
+                            "type": "wait",
+                            "data": {
+                              "waitTime": 7.0
+                            }
+                          },
+                          {
+                            "type": "named",
+                            "data": {
+                              "name": "Index"
+                            }
+                          }
+                        ]
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "URI Autos",
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/navgrid.json b/src/deploy/pathplanner/navgrid.json
index 6d5fbd8..ac5f521 100644
--- a/src/deploy/pathplanner/navgrid.json
+++ b/src/deploy/pathplanner/navgrid.json
@@ -1 +1 @@
-{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,true,true,false,true,true,true,true,true,true,true,true,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,true,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,true,true,true,false,false,false,false,false,true,true,true,true,true,true,false,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,true,true,false,false,false,false,false,true,true,true,true,true,true,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,true,true,false,false,false,false,false,true,true,true,true,true,true,false,true,false,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,false,false,false,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,false,true,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,true,false,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,false,false,false,false,true,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
+{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/C to C Tower.path b/src/deploy/pathplanner/paths/C to C Tower.path
new file mode 100644
index 0000000..c4f928b
--- /dev/null
+++ b/src/deploy/pathplanner/paths/C to C Tower.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.67039886039886,
+        "y": 4.073760683760685
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.7227499543102285,
+        "y": 3.75515588251785
+      },
+      "isLocked": false,
+      "linkedName": "Center Hub Start"
+    },
+    {
+      "anchor": {
+        "x": 1.7452849002849002,
+        "y": 3.7636752136752136
+      },
+      "prevControl": {
+        "x": 3.4722305952404113,
+        "y": 4.027101887287217
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Middle Tower"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/Depot to Left Tower.path b/src/deploy/pathplanner/paths/Depot to Left Tower.path
new file mode 100644
index 0000000..32b155e
--- /dev/null
+++ b/src/deploy/pathplanner/paths/Depot to Left Tower.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.5437037037037049,
+        "y": 5.921353276353277
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.177504552594503,
+        "y": 5.651689831885218
+      },
+      "isLocked": false,
+      "linkedName": "Depot Actual Intake Extended"
+    },
+    {
+      "anchor": {
+        "x": 1.4998005698005699,
+        "y": 5.081538461538462
+      },
+      "prevControl": {
+        "x": 1.0257205375021177,
+        "y": 5.804309201866249
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Left Tower"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 2.862405226111731
+  },
+  "reversed": false,
+  "folder": "Left Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/L Trench to Depot.path b/src/deploy/pathplanner/paths/L Trench to Depot.path
new file mode 100644
index 0000000..8ce196d
--- /dev/null
+++ b/src/deploy/pathplanner/paths/L Trench to Depot.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.631638176638176,
+        "y": 7.420099715099716
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.920295961536187,
+        "y": 7.382891735254048
+      },
+      "isLocked": false,
+      "linkedName": "Left Trench Start"
+    },
+    {
+      "anchor": {
+        "x": 0.5437037037037049,
+        "y": 5.921353276353277
+      },
+      "prevControl": {
+        "x": 0.18193732193732354,
+        "y": 8.091951566951568
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Depot Actual Intake Extended"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": "Left Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/Outpost Int Ext to Right Tower.path b/src/deploy/pathplanner/paths/Outpost Int Ext to Right Tower.path
new file mode 100644
index 0000000..44b0609
--- /dev/null
+++ b/src/deploy/pathplanner/paths/Outpost Int Ext to Right Tower.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.7633475783475787,
+        "y": 0.6369800569800577
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.6562592684805915,
+        "y": 2.5144924068122636
+      },
+      "isLocked": false,
+      "linkedName": "Outpost Intake Extended"
+    },
+    {
+      "anchor": {
+        "x": 1.6419230769230766,
+        "y": 2.4458119658119664
+      },
+      "prevControl": {
+        "x": 0.793772765376131,
+        "y": 0.7709484446254355
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Right Tower"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 22.963773059854557
+  },
+  "reversed": false,
+  "folder": "Right Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/paths/R Trench to Depot Intake Ext.path b/src/deploy/pathplanner/paths/R Trench to Depot Intake Ext.path
new file mode 100644
index 0000000..b48fca5
--- /dev/null
+++ b/src/deploy/pathplanner/paths/R Trench to Depot Intake Ext.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.6574786324786324,
+        "y": 0.6369800569800577
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.07838473844916,
+        "y": 0.6136361088335741
+      },
+      "isLocked": false,
+      "linkedName": "Right Trench Start"
+    },
+    {
+      "anchor": {
+        "x": 0.7633475783475787,
+        "y": 0.6369800569800577
+      },
+      "prevControl": {
+        "x": 2.001914989871157,
+        "y": 0.6413962249537501
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Outpost Intake Extended"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Right Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/settings.json b/src/deploy/pathplanner/settings.json
index c9aeeb6..7cdd769 100644
--- a/src/deploy/pathplanner/settings.json
+++ b/src/deploy/pathplanner/settings.json
@@ -1,30 +1,13 @@
 {
-  "robotWidth": 0.902,
-  "robotLength": 0.724,
+  "robotWidth": 0.9144,
+  "robotLength": 0.7366,
   "holonomicMode": true,
   "pathFolders": [
-    "LT Paths",
-    "MS Paths",
-    "Misc.",
-    "RT Paths",
-    "Return to Alliance",
-    "SUTO to Places"
+    "Left Paths",
+    "Right Paths"
   ],
   "autoFolders": [
-    "ATW DualShot",
-    "Brendan Wants It",
-    "Bump Auto",
-    "ChudBot Autos",
-    "LT Auditorium Auto",
-    "LT Stem Lab Auto",
-    "Locked Autos",
-    "MS Auditorium Auto",
-    "MS Stem Lab Auto",
-    "PeoplePleaser",
-    "RT Auditorium Auto",
-    "RT Stem Lab Auto",
-    "Repetitive Auto",
-    "Useless Auto"
+    "URI Autos"
   ],
   "defaultMaxVel": 3.0,
   "defaultMaxAccel": 3.0,
@@ -36,7 +19,7 @@
   "robotTrackwidth": 0.546,
   "driveWheelRadius": 0.051,
   "driveGearing": 5.273,
-  "maxDriveSpeed": 5.45,
+  "maxDriveSpeed": 5.85,
   "driveMotorType": "krakenX60",
   "driveCurrentLimit": 60.0,
   "wheelCOF": 1.2,
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
new file mode 100644
index 0000000..ac5f521
--- /dev/null
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -0,0 +1 @@
+{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file

From 61f3b5e0134caca85592bc92f134646cc16e0914 Mon Sep 17 00:00:00 2001
From: Maxwell Morgan 
Date: Thu, 19 Mar 2026 23:26:49 -0400
Subject: [PATCH 58/61] Update constants and pathplanner commands

---
 src/main/java/frc/robot/RobotContainer.java   | 103 +++++++++++-------
 .../frc/robot/control/DriverControls.java     |   4 +-
 .../subsystems/drive/DriveConstants.java      |   4 +-
 .../frc/robot/subsystems/indexer/Indexer.java |   8 +-
 .../robot/subsystems/leds/LedConstants.java   |   6 +-
 .../java/frc/robot/subsystems/leds/Leds.java  |  27 +----
 6 files changed, 71 insertions(+), 81 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index e3892ba..a3709da 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -11,9 +11,11 @@
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+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 frc.robot.Constants.Mode;
 import frc.robot.RobotState.OdometryObservation;
 import frc.robot.RobotState.VisionMeasurement;
 import frc.robot.control.Configurable;
@@ -43,10 +45,16 @@
 import frc.robot.subsystems.vision.Vision;
 import frc.robot.subsystems.vision.Vision.VisionConsumer;
 import frc.robot.util.GeomUtil;
+
+import static edu.wpi.first.units.Units.Seconds;
+
 import java.util.List;
 import java.util.function.Supplier;
 
+import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
+import com.pathplanner.lib.commands.PathPlannerAuto;
+import com.pathplanner.lib.path.PathPlannerPath;
 
 public class RobotContainer {
   private final DriverController driver = new DriverController.XboxDriverController(0);
@@ -61,6 +69,8 @@ public class RobotContainer {
   private static Field2d field2d = new Field2d();
   private static Field2d targetField2d = new Field2d();
 
+  private SendableChooser autoChooser = new SendableChooser<>();
+
   public RobotContainer() {
 
     Supplier robotRotationSupplier = () -> RobotState.getInstance().getRotation();
@@ -70,43 +80,40 @@ public RobotContainer() {
     field2d.setRobotPose(RobotState.getInstance().getEstimatedPose());
     switch (Constants.kCurrentMode) {
       case REAL -> {
-        drive =
-            new Drive(
-                new GyroIOPigeon2(),
-                new ModuleIOTalonFX(TunerConstants.FrontLeft),
-                new ModuleIOTalonFX(TunerConstants.FrontRight),
-                new ModuleIOTalonFX(TunerConstants.BackLeft),
-                new ModuleIOTalonFX(TunerConstants.BackRight));
+        drive = new Drive(
+            new GyroIOPigeon2(),
+            new ModuleIOTalonFX(TunerConstants.FrontLeft),
+            new ModuleIOTalonFX(TunerConstants.FrontRight),
+            new ModuleIOTalonFX(TunerConstants.BackLeft),
+            new ModuleIOTalonFX(TunerConstants.BackRight));
         indexer = new Indexer(new IndexerIOTalonFX());
         intake = new Intake(new IntakeIOTalonFX());
-        shooter =
-            new Shooter(new TurretIOSparkMax() {}, new HoodIOSparkMax(), new FlywheelIOTalonFX());
-        vision =
-            new Vision(
-                new VisionConsumer() {
-                  public void accept(
-                      Pose2d visionRobotPoseMeters,
-                      double timestampSeconds,
-                      edu.wpi.first.math.Matrix visionMeasurementStdDevs) {
-
-                    RobotState.getInstance()
-                        .addVisionMeasurement(
-                            new VisionMeasurement(
-                                timestampSeconds, visionRobotPoseMeters, visionMeasurementStdDevs));
-                  }
-                  ;
-                },
-                new CameraIOLimelight("limelight-front", robotRotationSupplier),
-                new CameraIOLimelight("limelight-one", robotRotationSupplier));
+        shooter = new Shooter(new TurretIOSparkMax() {
+        }, new HoodIOSparkMax(), new FlywheelIOTalonFX());
+        vision = new Vision(
+            new VisionConsumer() {
+              public void accept(
+                  Pose2d visionRobotPoseMeters,
+                  double timestampSeconds,
+                  edu.wpi.first.math.Matrix visionMeasurementStdDevs) {
+
+                RobotState.getInstance()
+                    .addVisionMeasurement(
+                        new VisionMeasurement(
+                            timestampSeconds, visionRobotPoseMeters, visionMeasurementStdDevs));
+              };
+            },
+            new CameraIOLimelight("limelight-front", robotRotationSupplier),
+            new CameraIOLimelight("limelight-one", robotRotationSupplier));
       }
       case SIM -> {
-        drive =
-            new Drive(
-                new GyroIO() {},
-                new ModuleIOSim(TunerConstants.FrontLeft),
-                new ModuleIOSim(TunerConstants.FrontRight),
-                new ModuleIOSim(TunerConstants.BackLeft),
-                new ModuleIOSim(TunerConstants.BackRight));
+        drive = new Drive(
+            new GyroIO() {
+            },
+            new ModuleIOSim(TunerConstants.FrontLeft),
+            new ModuleIOSim(TunerConstants.FrontRight),
+            new ModuleIOSim(TunerConstants.BackLeft),
+            new ModuleIOSim(TunerConstants.BackRight));
         indexer = new Indexer(new IndexerIOSim());
         intake = new Intake(new IntakeIOSim());
         shooter = new Shooter(new TurretIOSim(), new HoodIOSim(), new FlywheelIOSim());
@@ -114,12 +121,20 @@ public void accept(
     }
 
     configureBindings();
+
+    if (Constants.kCurrentMode == Mode.REAL) {
+      configurePathPlanner();
+
+      autoChooser = AutoBuilder.buildAutoChooser();
+
+      SmartDashboard.putData(autoChooser);
+    }
   }
 
   private void configureBindings() {
     List.of(
-            new DefaultControls(driver, operator, drive, indexer, intake, shooter),
-            new DriverControls(driver, operator, drive, shooter, intake, indexer))
+        new DefaultControls(driver, operator, drive, indexer, intake, shooter),
+        new DriverControls(driver, operator, drive, shooter, intake, indexer))
         .forEach(Configurable::configure);
   }
 
@@ -129,10 +144,10 @@ public void robotPeriodic() {
             new OdometryObservation(
                 Timer.getTimestamp(),
                 new SwerveModulePosition[] {
-                  new SwerveModulePosition(),
-                  new SwerveModulePosition(),
-                  new SwerveModulePosition(),
-                  new SwerveModulePosition()
+                    new SwerveModulePosition(),
+                    new SwerveModulePosition(),
+                    new SwerveModulePosition(),
+                    new SwerveModulePosition()
                 },
                 drive.getRawGyroRotation()));
 
@@ -141,13 +156,17 @@ public void robotPeriodic() {
   }
 
   public Command getAutonomousCommand() {
+    if (Constants.kCurrentMode == Mode.REAL) {
+      return autoChooser.getSelected();
+    }
     return Commands.print("No autonomous command configured");
   }
 
   public void configurePathPlanner() {
-    NamedCommands.registerCommand("Shoot at Hub/Pass", shooter.shootAtTargetNoRotation(() -> RobotState.getInstance().getTurretTarget()));
+    NamedCommands.registerCommand("Shoot", shooter.shootAtTargetNoRotation(() -> RobotState.getInstance().getTurretTarget()));
     NamedCommands.registerCommand("Index", indexer.index());
-    NamedCommands.registerCommand("Intake Deploy", intake.deployOpenLoop());
-    NamedCommands.registerCommand("Intake Retract", intake.retractOpenLoop());
+    NamedCommands.registerCommand("Intake", intake.intake());
+    NamedCommands.registerCommand("DeployIntake", intake.deployOpenLoop().withTimeout(Seconds.of(2)));
+    NamedCommands.registerCommand("RetractIntake", intake.retractOpenLoop().withTimeout(Seconds.of(2)));
    }
 }
diff --git a/src/main/java/frc/robot/control/DriverControls.java b/src/main/java/frc/robot/control/DriverControls.java
index d4bd8b4..ef0e346 100644
--- a/src/main/java/frc/robot/control/DriverControls.java
+++ b/src/main/java/frc/robot/control/DriverControls.java
@@ -133,8 +133,8 @@ private void configureSingleController() {
     // () -> shooter.setFlywheelOpenLoop(0),
     // shooter));
 
-    driver.aCross().whileTrue(indexer.index());
-    driver.bCircle().whileTrue(indexer.indexReverse());
+    driver.bCircle().whileTrue(indexer.index());
+    driver.aCross().whileTrue(indexer.indexReverse());
 
     // driver
     // .aCross()
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
index c973310..c0b460f 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -58,14 +58,14 @@ public final class DriveConstants {
 
   // TODO: Update for robot
   // PathPlanner config constants
-  public static final double kRobotMassKG = 74.088;
+  public static final double kRobotMassKG = 72.088;
   public static final double kRobotMOI = 6.883;
   /** Coefficient of friction */
   public static final double kWheelCOF = 1.2;
 
   public static final RobotConfig kPathplannerConfig =
       new RobotConfig(
-          kRobotMOI,
+          kRobotMassKG,
           kRobotMOI,
           new ModuleConfig(
               TunerConstants.FrontLeft.WheelRadius,
diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java
index 4028103..d919ba5 100644
--- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java
+++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java
@@ -27,8 +27,8 @@ public void periodic() {
   public Command index() {
     return Commands.startEnd(
         () -> {
-          io.setThroatOpenLoop(IndexerConstants.kThroatMotorSpeed);
-          io.setToungeOpenLoop(-IndexerConstants.kThroatMotorSpeed);
+          io.setThroatOpenLoop(-IndexerConstants.kThroatMotorSpeed);
+          io.setToungeOpenLoop(IndexerConstants.kThroatMotorSpeed);
         },
         () -> {
           io.stop();
@@ -39,8 +39,8 @@ public Command index() {
   public Command indexReverse() {
     return Commands.startEnd(
         () -> {
-          io.setThroatOpenLoop(-IndexerConstants.kThroatMotorSpeed);
-          io.setToungeOpenLoop(IndexerConstants.kThroatMotorSpeed);
+          io.setThroatOpenLoop(IndexerConstants.kThroatMotorSpeed);
+          io.setToungeOpenLoop(-IndexerConstants.kThroatMotorSpeed);
         },
         () -> {
           io.stop();
diff --git a/src/main/java/frc/robot/subsystems/leds/LedConstants.java b/src/main/java/frc/robot/subsystems/leds/LedConstants.java
index c2a5ab1..2457d2b 100644
--- a/src/main/java/frc/robot/subsystems/leds/LedConstants.java
+++ b/src/main/java/frc/robot/subsystems/leds/LedConstants.java
@@ -3,11 +3,7 @@
 public final class LedConstants {
   public static final int kPort = 1;
 
-  public static final int kFullLength = 14;
-  public static final int kLeftTurretBottomLength = 7;
-  public static final int kRightTurretBottomLength = 7;
-  public static final int kLeftTurretTopLength = 17;
-  public static final int kRightTurretTopLength = 15;
+  public static final int kFullLength = 5;
 
   public static final double kStartupBreathDuration = 1.0;
   public static final double kStrobeSlowDuration = 0.2;
diff --git a/src/main/java/frc/robot/subsystems/leds/Leds.java b/src/main/java/frc/robot/subsystems/leds/Leds.java
index 4b3c005..b09b633 100644
--- a/src/main/java/frc/robot/subsystems/leds/Leds.java
+++ b/src/main/java/frc/robot/subsystems/leds/Leds.java
@@ -22,32 +22,7 @@ public static Leds getInstance() {
   public record Section(int start, int end) {}
 
   public enum LedSection {
-    ALL(new Section(0, LedConstants.kFullLength - 1)),
-    ALL_LEFT(
-        new Section(
-            0, LedConstants.kLeftTurretBottomLength + LedConstants.kLeftTurretTopLength - 1)),
-    ALL_RIGHT(
-        new Section(
-            LedConstants.kLeftTurretBottomLength + LedConstants.kLeftTurretTopLength,
-            LedConstants.kFullLength)),
-    BOTTOM_LEFT_TURRET(new Section(0, LedConstants.kLeftTurretBottomLength - 1)),
-    TOP_LEFT_TURRET(
-        new Section(
-            LedConstants.kLeftTurretBottomLength,
-            LedConstants.kLeftTurretBottomLength + LedConstants.kLeftTurretTopLength - 1)),
-    TOP_RIGHT_TURRET(
-        new Section(
-            LedConstants.kLeftTurretBottomLength + LedConstants.kLeftTurretTopLength,
-            LedConstants.kLeftTurretBottomLength
-                + LedConstants.kLeftTurretTopLength
-                + LedConstants.kRightTurretTopLength
-                - 1)),
-    BOTTOM_RIGHT_TURRET(
-        new Section(
-            LedConstants.kLeftTurretBottomLength
-                + LedConstants.kLeftTurretTopLength
-                + LedConstants.kRightTurretTopLength,
-            LedConstants.kFullLength));
+    ALL(new Section(0, LedConstants.kFullLength - 1));
 
     private final Section section;
 

From 23ad59f0795a7d010e99751307aaacbd209f9ddb Mon Sep 17 00:00:00 2001
From: Legion 
Date: Fri, 20 Mar 2026 10:35:47 -0400
Subject: [PATCH 59/61] Fixed pathplanner position in files. Should work.

---
 src/deploy/pathplanner/navgrid.json                              | 1 -
 .../deploy/pathplanner/autos/ 1678 Replica Auto RT (2).auto      | 0
 .../pathplanner/autos/ DualShot RT Round the World to OP.auto    | 0
 .../pathplanner/autos/ Half RT Round the World - ASSIST.auto     | 0
 .../deploy/pathplanner/autos/ LT - BPTL - SUTO - OP.auto         | 0
 .../deploy/pathplanner/autos/1678 Replica Auto LT (1) .auto      | 0
 .../deploy/pathplanner/autos/1678 Replica Auto LT (2) .auto      | 0
 .../deploy/pathplanner/autos/1678 Replica Auto RT (1).auto       | 0
 src/{ => main}/deploy/pathplanner/autos/8 Auto LT.auto           | 0
 src/{ => main}/deploy/pathplanner/autos/8 Auto RT.auto           | 0
 ...rendan's stupid stupid stupid auto but on the other side.auto | 0
 .../pathplanner/autos/Brendan's stupid stupid stupid auto.auto   | 0
 .../pathplanner/autos/DualShot LT Round the World to DP.auto     | 0
 .../pathplanner/autos/DualShot LT Round the World to OP.auto     | 0
 .../pathplanner/autos/DualShot MS Round the World to Climb.auto  | 0
 .../pathplanner/autos/DualShot MS Round the World to DP.auto     | 0
 .../pathplanner/autos/DualShot MS Round the World to OP.auto     | 0
 .../pathplanner/autos/DualShot RT Round the World to Climb.auto  | 0
 .../pathplanner/autos/DualShot RT Round the World to DP.auto     | 0
 .../pathplanner/autos/Dualshot LT Round the World to Climb.auto  | 0
 .../pathplanner/autos/Half LT Round the World - ASSIST.auto      | 0
 .../pathplanner/autos/Half LT Round the World - HOARD.auto       | 0
 .../deploy/pathplanner/autos/Half RT Round the World - HOAR.auto | 0
 .../deploy/pathplanner/autos/LT  - OP - SUTO - Climb.auto        | 0
 .../deploy/pathplanner/autos/LT - BPTL - SUTO - DP.auto          | 0
 .../deploy/pathplanner/autos/LT - DP - SUTO - Climb.auto         | 0
 .../deploy/pathplanner/autos/LT - DP - SUTO - OP .auto           | 0
 .../deploy/pathplanner/autos/LT - RT - SUTO Bump Auto.auto       | 0
 .../deploy/pathplanner/autos/LT - RT - SUTO Bump Climb Auto.auto | 0
 src/{ => main}/deploy/pathplanner/autos/LT Locked Auto.auto      | 0
 src/{ => main}/deploy/pathplanner/autos/LT Repetitive.auto       | 0
 .../deploy/pathplanner/autos/LT Round the World to Climb.auto    | 0
 .../deploy/pathplanner/autos/LT Round the World to DP.auto       | 0
 .../deploy/pathplanner/autos/LT Round the World to OP.auto       | 0
 src/{ => main}/deploy/pathplanner/autos/LT to RT MoveShot.auto   | 0
 .../deploy/pathplanner/autos/MS - DP - SUTO - Climb .auto        | 0
 .../pathplanner/autos/MS - DP - SUTO - OP (No Intake).auto       | 0
 .../deploy/pathplanner/autos/MS - LT - RT - SUTO Bump Auto.auto  | 0
 .../pathplanner/autos/MS - LT - RT - SUTO Bump Climb Auto.auto   | 0
 src/{ => main}/deploy/pathplanner/autos/MS - LT MoveShot.auto    | 0
 src/{ => main}/deploy/pathplanner/autos/MS - LT Repetitive.auto  | 0
 .../deploy/pathplanner/autos/MS - OP - SUTO - Climb.auto         | 0
 src/{ => main}/deploy/pathplanner/autos/MS - RT MoveShot.auto    | 0
 src/{ => main}/deploy/pathplanner/autos/MS - RT Repetitive.auto  | 0
 .../deploy/pathplanner/autos/MS Round the World to Climb.auto    | 0
 .../deploy/pathplanner/autos/MS Round the World to DP.auto       | 0
 .../deploy/pathplanner/autos/MS Round the World to OP.auto       | 0
 .../deploy/pathplanner/autos/MS-LT Round the World - ASSIST.auto | 0
 .../deploy/pathplanner/autos/MS-LT Round the World - HOARD.auto  | 0
 .../deploy/pathplanner/autos/MS-RT Round the World - ASSIST.auto | 0
 .../deploy/pathplanner/autos/MS-RT Round the World - HOAR.auto   | 0
 .../deploy/pathplanner/autos/RT - BPBR - SUTO - DP.auto          | 0
 .../deploy/pathplanner/autos/RT - BPBR - SUTO - OP.auto          | 0
 .../deploy/pathplanner/autos/RT - DP - SUTO - Climb.auto         | 0
 .../deploy/pathplanner/autos/RT - DP - SUTO - OP .auto           | 0
 .../deploy/pathplanner/autos/RT - LT - SUTO Bump Auto.auto       | 0
 .../deploy/pathplanner/autos/RT - LT - SUTO Bump Climb Auto.auto | 0
 src/{ => main}/deploy/pathplanner/autos/RT - LT MoveShot.auto    | 0
 .../deploy/pathplanner/autos/RT - OP - SUTO - Climb.auto         | 0
 src/{ => main}/deploy/pathplanner/autos/RT Locked Auto.auto      | 0
 src/{ => main}/deploy/pathplanner/autos/RT Repetitive.auto       | 0
 .../deploy/pathplanner/autos/RT Round the World to Climb.auto    | 0
 .../deploy/pathplanner/autos/RT Round the World to DP.auto       | 0
 .../deploy/pathplanner/autos/RT Round the World to OP.auto       | 0
 src/{ => main}/deploy/pathplanner/autos/URI Center.auto          | 0
 src/{ => main}/deploy/pathplanner/autos/URI Left Depot.auto      | 0
 src/{ => main}/deploy/pathplanner/autos/URI Right Outpost.auto   | 0
 .../pathplanner/paths/ 8 Point to ACTUALMiddleBallPit.path       | 0
 .../deploy/pathplanner/paths/ LT Corner 3 to MidBPTL (2).path    | 0
 src/{ => main}/deploy/pathplanner/paths/ MS - RTBump.path        | 0
 .../deploy/pathplanner/paths/ RT Corner 3 - MidBPBR (2).path     | 0
 src/{ => main}/deploy/pathplanner/paths/8Point to MidBPTL.path   | 0
 .../deploy/pathplanner/paths/ACTUALMiddle to RTBump.path         | 0
 .../deploy/pathplanner/paths/ACTUALMiddleBallPit to BPTL.path    | 0
 .../deploy/pathplanner/paths/ACTUALMiddleBallPit to LT.path      | 0
 .../deploy/pathplanner/paths/ACTUALMiddleBallPit to LTBump.path  | 0
 .../deploy/pathplanner/paths/ACTUALMiddleBallPit to RT.path      | 0
 .../deploy/pathplanner/paths/ACTUALMiddleBallPit2 to BPBR.path   | 0
 src/{ => main}/deploy/pathplanner/paths/BPBR - RTBump.path       | 0
 .../deploy/pathplanner/paths/BPBR to MiddleBallPit.path          | 0
 src/{ => main}/deploy/pathplanner/paths/BPBR to RT.path          | 0
 src/{ => main}/deploy/pathplanner/paths/BPBR to RTBump.path      | 0
 src/{ => main}/deploy/pathplanner/paths/BPBR to RTCorner3.path   | 0
 .../deploy/pathplanner/paths/BPTL to ACTUALMiddleversion2.path   | 0
 src/{ => main}/deploy/pathplanner/paths/BPTL to LT.path          | 0
 .../deploy/pathplanner/paths/Bottom to Rotated Top.path          | 0
 src/{ => main}/deploy/pathplanner/paths/Bottom to Top.path       | 0
 src/{ => main}/deploy/pathplanner/paths/C to C Tower.path        | 0
 src/{ => main}/deploy/pathplanner/paths/CornerLine LT to DP.path | 0
 src/{ => main}/deploy/pathplanner/paths/DP to Climb.path         | 0
 src/{ => main}/deploy/pathplanner/paths/DP to SUTO.path          | 0
 src/{ => main}/deploy/pathplanner/paths/Depot to Left Tower.path | 0
 .../deploy/pathplanner/paths/Fadeaway Top to Bottom.path         | 0
 src/{ => main}/deploy/pathplanner/paths/L Trench to Depot.path   | 0
 src/{ => main}/deploy/pathplanner/paths/LT - MidBPTL.path        | 0
 .../deploy/pathplanner/paths/LT Corner 3 to 8Point.path          | 0
 src/{ => main}/deploy/pathplanner/paths/LT Corner 3 to DP.path   | 0
 .../pathplanner/paths/LT Corner 3 to MidBPTL (Sped Up).path      | 0
 .../deploy/pathplanner/paths/LT Corner 3 to MidBPTL.path         | 0
 src/{ => main}/deploy/pathplanner/paths/LT Corner 3 to SUTO.path | 0
 .../deploy/pathplanner/paths/LT CornerLine to SUTO.path          | 0
 src/{ => main}/deploy/pathplanner/paths/LT To PeakSUTO.path      | 0
 src/{ => main}/deploy/pathplanner/paths/LT to BPTL.path          | 0
 src/{ => main}/deploy/pathplanner/paths/LT to Climb.path         | 0
 src/{ => main}/deploy/pathplanner/paths/LT to DP.path            | 0
 src/{ => main}/deploy/pathplanner/paths/LT to LTBump26.path      | 0
 src/{ => main}/deploy/pathplanner/paths/LT to OP.path            | 0
 src/{ => main}/deploy/pathplanner/paths/LT to Rotated BPTL.path  | 0
 src/{ => main}/deploy/pathplanner/paths/LT to Shoot.path         | 0
 src/{ => main}/deploy/pathplanner/paths/LTBump - BPTL.path       | 0
 src/{ => main}/deploy/pathplanner/paths/LTBump to DP.path        | 0
 .../deploy/pathplanner/paths/LTBump to LTCorner 3.path           | 0
 src/{ => main}/deploy/pathplanner/paths/LTBump to SUTO.path      | 0
 src/{ => main}/deploy/pathplanner/paths/MS - LT Corner 3.path    | 0
 src/{ => main}/deploy/pathplanner/paths/MS - LTBump.path         | 0
 src/{ => main}/deploy/pathplanner/paths/MS - RT Corner 3.path    | 0
 src/{ => main}/deploy/pathplanner/paths/MS to Climb.path         | 0
 src/{ => main}/deploy/pathplanner/paths/MS to DP.path            | 0
 src/{ => main}/deploy/pathplanner/paths/MS to LT.path            | 0
 src/{ => main}/deploy/pathplanner/paths/MS to OP.path            | 0
 src/{ => main}/deploy/pathplanner/paths/MS to RT.path            | 0
 .../deploy/pathplanner/paths/MidBPBR - RT Corner 3.path          | 0
 .../deploy/pathplanner/paths/MidBPTL - LT Corner 3.path          | 0
 src/{ => main}/deploy/pathplanner/paths/OP to SUTO.path          | 0
 .../deploy/pathplanner/paths/Outpost Int Ext to Right Tower.path | 0
 .../deploy/pathplanner/paths/R Trench to Depot Intake Ext.path   | 0
 src/{ => main}/deploy/pathplanner/paths/RT - MidBPBR.path        | 0
 src/{ => main}/deploy/pathplanner/paths/RT - PeakSUTO.path       | 0
 .../pathplanner/paths/RT 8 Point to ACTUALMiddleBallPit2.path    | 0
 .../pathplanner/paths/RT Corner 3 - MidBPBR (2) Sped Up.path     | 0
 .../pathplanner/paths/RT Corner 3 - MidBPBR (Sped Up).path       | 0
 .../deploy/pathplanner/paths/RT Corner 3 - MidBPBR.path          | 0
 .../deploy/pathplanner/paths/RT Corner 3 ro RT 8 Point.path      | 0
 src/{ => main}/deploy/pathplanner/paths/RT Corner 3 to OP.path   | 0
 src/{ => main}/deploy/pathplanner/paths/RT Corner3 to SUTO.path  | 0
 src/{ => main}/deploy/pathplanner/paths/RT Corner3-DP.path       | 0
 src/{ => main}/deploy/pathplanner/paths/RT to Climb.path         | 0
 src/{ => main}/deploy/pathplanner/paths/RT to DP.path            | 0
 src/{ => main}/deploy/pathplanner/paths/RT to OP.path            | 0
 src/{ => main}/deploy/pathplanner/paths/RT to RTBump26.path      | 0
 src/{ => main}/deploy/pathplanner/paths/RT to Rotated BPBR.path  | 0
 src/{ => main}/deploy/pathplanner/paths/RT to Shoot.path         | 0
 src/{ => main}/deploy/pathplanner/paths/RTBump - BPBR.path       | 0
 src/{ => main}/deploy/pathplanner/paths/RTBump to OP.path        | 0
 .../deploy/pathplanner/paths/RTBump to RTCorner 3.path           | 0
 src/{ => main}/deploy/pathplanner/paths/RTBump to SUTO.path      | 0
 .../deploy/pathplanner/paths/Rotated BPBR to RT Corner 3.path    | 0
 src/{ => main}/deploy/pathplanner/paths/Rotated BPBR to RT.path  | 0
 .../deploy/pathplanner/paths/Rotated BPTL - LTBump.path          | 0
 .../deploy/pathplanner/paths/Rotated BPTL to LT Corrner 3.path   | 0
 src/{ => main}/deploy/pathplanner/paths/Rotated BPTL to LT.path  | 0
 .../deploy/pathplanner/paths/RotatedBPBR to RT Corner 3.path     | 0
 .../deploy/pathplanner/paths/RotatedBPTL to LT Corner 3.path     | 0
 src/{ => main}/deploy/pathplanner/paths/SUTO - OP.path           | 0
 src/{ => main}/deploy/pathplanner/paths/SUTO to Climb.path       | 0
 src/{ => main}/deploy/pathplanner/paths/SUTO to DP.path          | 0
 src/{ => main}/deploy/pathplanner/paths/SUTO to LT.path          | 0
 src/{ => main}/deploy/pathplanner/paths/SUTO to RT.path          | 0
 src/{ => main}/deploy/pathplanner/paths/Top to Bottom.path       | 0
 src/{ => main}/deploy/pathplanner/settings.json                  | 0
 160 files changed, 1 deletion(-)
 delete mode 100644 src/deploy/pathplanner/navgrid.json
 rename src/{ => main}/deploy/pathplanner/autos/ 1678 Replica Auto RT (2).auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/ DualShot RT Round the World to OP.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/ Half RT Round the World - ASSIST.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/ LT - BPTL - SUTO - OP.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/1678 Replica Auto LT (1) .auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/1678 Replica Auto LT (2) .auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/1678 Replica Auto RT (1).auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/8 Auto LT.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/8 Auto RT.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/Brendan's stupid stupid stupid auto but on the other side.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/Brendan's stupid stupid stupid auto.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/DualShot LT Round the World to DP.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/DualShot LT Round the World to OP.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/DualShot MS Round the World to Climb.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/DualShot MS Round the World to DP.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/DualShot MS Round the World to OP.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/DualShot RT Round the World to Climb.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/DualShot RT Round the World to DP.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/Dualshot LT Round the World to Climb.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/Half LT Round the World - ASSIST.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/Half LT Round the World - HOARD.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/Half RT Round the World - HOAR.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/LT  - OP - SUTO - Climb.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/LT - BPTL - SUTO - DP.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/LT - DP - SUTO - Climb.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/LT - DP - SUTO - OP .auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/LT - RT - SUTO Bump Auto.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/LT - RT - SUTO Bump Climb Auto.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/LT Locked Auto.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/LT Repetitive.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/LT Round the World to Climb.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/LT Round the World to DP.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/LT Round the World to OP.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/LT to RT MoveShot.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/MS - DP - SUTO - Climb .auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/MS - DP - SUTO - OP (No Intake).auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/MS - LT - RT - SUTO Bump Auto.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/MS - LT - RT - SUTO Bump Climb Auto.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/MS - LT MoveShot.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/MS - LT Repetitive.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/MS - OP - SUTO - Climb.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/MS - RT MoveShot.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/MS - RT Repetitive.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/MS Round the World to Climb.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/MS Round the World to DP.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/MS Round the World to OP.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/MS-LT Round the World - ASSIST.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/MS-LT Round the World - HOARD.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/MS-RT Round the World - ASSIST.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/MS-RT Round the World - HOAR.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/RT - BPBR - SUTO - DP.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/RT - BPBR - SUTO - OP.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/RT - DP - SUTO - Climb.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/RT - DP - SUTO - OP .auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/RT - LT - SUTO Bump Auto.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/RT - LT - SUTO Bump Climb Auto.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/RT - LT MoveShot.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/RT - OP - SUTO - Climb.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/RT Locked Auto.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/RT Repetitive.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/RT Round the World to Climb.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/RT Round the World to DP.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/RT Round the World to OP.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/URI Center.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/URI Left Depot.auto (100%)
 rename src/{ => main}/deploy/pathplanner/autos/URI Right Outpost.auto (100%)
 rename src/{ => main}/deploy/pathplanner/paths/ 8 Point to ACTUALMiddleBallPit.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/ LT Corner 3 to MidBPTL (2).path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/ MS - RTBump.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/ RT Corner 3 - MidBPBR (2).path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/8Point to MidBPTL.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/ACTUALMiddle to RTBump.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/ACTUALMiddleBallPit to BPTL.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/ACTUALMiddleBallPit to LT.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/ACTUALMiddleBallPit to LTBump.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/ACTUALMiddleBallPit to RT.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/ACTUALMiddleBallPit2 to BPBR.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/BPBR - RTBump.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/BPBR to MiddleBallPit.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/BPBR to RT.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/BPBR to RTBump.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/BPBR to RTCorner3.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/BPTL to ACTUALMiddleversion2.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/BPTL to LT.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/Bottom to Rotated Top.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/Bottom to Top.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/C to C Tower.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/CornerLine LT to DP.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/DP to Climb.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/DP to SUTO.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/Depot to Left Tower.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/Fadeaway Top to Bottom.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/L Trench to Depot.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/LT - MidBPTL.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/LT Corner 3 to 8Point.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/LT Corner 3 to DP.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/LT Corner 3 to MidBPTL (Sped Up).path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/LT Corner 3 to MidBPTL.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/LT Corner 3 to SUTO.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/LT CornerLine to SUTO.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/LT To PeakSUTO.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/LT to BPTL.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/LT to Climb.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/LT to DP.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/LT to LTBump26.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/LT to OP.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/LT to Rotated BPTL.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/LT to Shoot.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/LTBump - BPTL.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/LTBump to DP.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/LTBump to LTCorner 3.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/LTBump to SUTO.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/MS - LT Corner 3.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/MS - LTBump.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/MS - RT Corner 3.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/MS to Climb.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/MS to DP.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/MS to LT.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/MS to OP.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/MS to RT.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/MidBPBR - RT Corner 3.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/MidBPTL - LT Corner 3.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/OP to SUTO.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/Outpost Int Ext to Right Tower.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/R Trench to Depot Intake Ext.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RT - MidBPBR.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RT - PeakSUTO.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RT 8 Point to ACTUALMiddleBallPit2.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RT Corner 3 - MidBPBR (2) Sped Up.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RT Corner 3 - MidBPBR (Sped Up).path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RT Corner 3 - MidBPBR.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RT Corner 3 ro RT 8 Point.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RT Corner 3 to OP.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RT Corner3 to SUTO.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RT Corner3-DP.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RT to Climb.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RT to DP.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RT to OP.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RT to RTBump26.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RT to Rotated BPBR.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RT to Shoot.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RTBump - BPBR.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RTBump to OP.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RTBump to RTCorner 3.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RTBump to SUTO.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/Rotated BPBR to RT Corner 3.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/Rotated BPBR to RT.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/Rotated BPTL - LTBump.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/Rotated BPTL to LT Corrner 3.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/Rotated BPTL to LT.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RotatedBPBR to RT Corner 3.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/RotatedBPTL to LT Corner 3.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/SUTO - OP.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/SUTO to Climb.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/SUTO to DP.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/SUTO to LT.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/SUTO to RT.path (100%)
 rename src/{ => main}/deploy/pathplanner/paths/Top to Bottom.path (100%)
 rename src/{ => main}/deploy/pathplanner/settings.json (100%)

diff --git a/src/deploy/pathplanner/navgrid.json b/src/deploy/pathplanner/navgrid.json
deleted file mode 100644
index ac5f521..0000000
--- a/src/deploy/pathplanner/navgrid.json
+++ /dev/null
@@ -1 +0,0 @@
-{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/deploy/pathplanner/autos/ 1678 Replica Auto RT (2).auto b/src/main/deploy/pathplanner/autos/ 1678 Replica Auto RT (2).auto
similarity index 100%
rename from src/deploy/pathplanner/autos/ 1678 Replica Auto RT (2).auto
rename to src/main/deploy/pathplanner/autos/ 1678 Replica Auto RT (2).auto
diff --git a/src/deploy/pathplanner/autos/ DualShot RT Round the World to OP.auto b/src/main/deploy/pathplanner/autos/ DualShot RT Round the World to OP.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/ DualShot RT Round the World to OP.auto
rename to src/main/deploy/pathplanner/autos/ DualShot RT Round the World to OP.auto
diff --git a/src/deploy/pathplanner/autos/ Half RT Round the World - ASSIST.auto b/src/main/deploy/pathplanner/autos/ Half RT Round the World - ASSIST.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/ Half RT Round the World - ASSIST.auto
rename to src/main/deploy/pathplanner/autos/ Half RT Round the World - ASSIST.auto
diff --git a/src/deploy/pathplanner/autos/ LT - BPTL - SUTO - OP.auto b/src/main/deploy/pathplanner/autos/ LT - BPTL - SUTO - OP.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/ LT - BPTL - SUTO - OP.auto
rename to src/main/deploy/pathplanner/autos/ LT - BPTL - SUTO - OP.auto
diff --git a/src/deploy/pathplanner/autos/1678 Replica Auto LT (1) .auto b/src/main/deploy/pathplanner/autos/1678 Replica Auto LT (1) .auto
similarity index 100%
rename from src/deploy/pathplanner/autos/1678 Replica Auto LT (1) .auto
rename to src/main/deploy/pathplanner/autos/1678 Replica Auto LT (1) .auto
diff --git a/src/deploy/pathplanner/autos/1678 Replica Auto LT (2) .auto b/src/main/deploy/pathplanner/autos/1678 Replica Auto LT (2) .auto
similarity index 100%
rename from src/deploy/pathplanner/autos/1678 Replica Auto LT (2) .auto
rename to src/main/deploy/pathplanner/autos/1678 Replica Auto LT (2) .auto
diff --git a/src/deploy/pathplanner/autos/1678 Replica Auto RT (1).auto b/src/main/deploy/pathplanner/autos/1678 Replica Auto RT (1).auto
similarity index 100%
rename from src/deploy/pathplanner/autos/1678 Replica Auto RT (1).auto
rename to src/main/deploy/pathplanner/autos/1678 Replica Auto RT (1).auto
diff --git a/src/deploy/pathplanner/autos/8 Auto LT.auto b/src/main/deploy/pathplanner/autos/8 Auto LT.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/8 Auto LT.auto
rename to src/main/deploy/pathplanner/autos/8 Auto LT.auto
diff --git a/src/deploy/pathplanner/autos/8 Auto RT.auto b/src/main/deploy/pathplanner/autos/8 Auto RT.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/8 Auto RT.auto
rename to src/main/deploy/pathplanner/autos/8 Auto RT.auto
diff --git a/src/deploy/pathplanner/autos/Brendan's stupid stupid stupid auto but on the other side.auto b/src/main/deploy/pathplanner/autos/Brendan's stupid stupid stupid auto but on the other side.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/Brendan's stupid stupid stupid auto but on the other side.auto
rename to src/main/deploy/pathplanner/autos/Brendan's stupid stupid stupid auto but on the other side.auto
diff --git a/src/deploy/pathplanner/autos/Brendan's stupid stupid stupid auto.auto b/src/main/deploy/pathplanner/autos/Brendan's stupid stupid stupid auto.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/Brendan's stupid stupid stupid auto.auto
rename to src/main/deploy/pathplanner/autos/Brendan's stupid stupid stupid auto.auto
diff --git a/src/deploy/pathplanner/autos/DualShot LT Round the World to DP.auto b/src/main/deploy/pathplanner/autos/DualShot LT Round the World to DP.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/DualShot LT Round the World to DP.auto
rename to src/main/deploy/pathplanner/autos/DualShot LT Round the World to DP.auto
diff --git a/src/deploy/pathplanner/autos/DualShot LT Round the World to OP.auto b/src/main/deploy/pathplanner/autos/DualShot LT Round the World to OP.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/DualShot LT Round the World to OP.auto
rename to src/main/deploy/pathplanner/autos/DualShot LT Round the World to OP.auto
diff --git a/src/deploy/pathplanner/autos/DualShot MS Round the World to Climb.auto b/src/main/deploy/pathplanner/autos/DualShot MS Round the World to Climb.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/DualShot MS Round the World to Climb.auto
rename to src/main/deploy/pathplanner/autos/DualShot MS Round the World to Climb.auto
diff --git a/src/deploy/pathplanner/autos/DualShot MS Round the World to DP.auto b/src/main/deploy/pathplanner/autos/DualShot MS Round the World to DP.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/DualShot MS Round the World to DP.auto
rename to src/main/deploy/pathplanner/autos/DualShot MS Round the World to DP.auto
diff --git a/src/deploy/pathplanner/autos/DualShot MS Round the World to OP.auto b/src/main/deploy/pathplanner/autos/DualShot MS Round the World to OP.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/DualShot MS Round the World to OP.auto
rename to src/main/deploy/pathplanner/autos/DualShot MS Round the World to OP.auto
diff --git a/src/deploy/pathplanner/autos/DualShot RT Round the World to Climb.auto b/src/main/deploy/pathplanner/autos/DualShot RT Round the World to Climb.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/DualShot RT Round the World to Climb.auto
rename to src/main/deploy/pathplanner/autos/DualShot RT Round the World to Climb.auto
diff --git a/src/deploy/pathplanner/autos/DualShot RT Round the World to DP.auto b/src/main/deploy/pathplanner/autos/DualShot RT Round the World to DP.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/DualShot RT Round the World to DP.auto
rename to src/main/deploy/pathplanner/autos/DualShot RT Round the World to DP.auto
diff --git a/src/deploy/pathplanner/autos/Dualshot LT Round the World to Climb.auto b/src/main/deploy/pathplanner/autos/Dualshot LT Round the World to Climb.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/Dualshot LT Round the World to Climb.auto
rename to src/main/deploy/pathplanner/autos/Dualshot LT Round the World to Climb.auto
diff --git a/src/deploy/pathplanner/autos/Half LT Round the World - ASSIST.auto b/src/main/deploy/pathplanner/autos/Half LT Round the World - ASSIST.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/Half LT Round the World - ASSIST.auto
rename to src/main/deploy/pathplanner/autos/Half LT Round the World - ASSIST.auto
diff --git a/src/deploy/pathplanner/autos/Half LT Round the World - HOARD.auto b/src/main/deploy/pathplanner/autos/Half LT Round the World - HOARD.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/Half LT Round the World - HOARD.auto
rename to src/main/deploy/pathplanner/autos/Half LT Round the World - HOARD.auto
diff --git a/src/deploy/pathplanner/autos/Half RT Round the World - HOAR.auto b/src/main/deploy/pathplanner/autos/Half RT Round the World - HOAR.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/Half RT Round the World - HOAR.auto
rename to src/main/deploy/pathplanner/autos/Half RT Round the World - HOAR.auto
diff --git a/src/deploy/pathplanner/autos/LT  - OP - SUTO - Climb.auto b/src/main/deploy/pathplanner/autos/LT  - OP - SUTO - Climb.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/LT  - OP - SUTO - Climb.auto
rename to src/main/deploy/pathplanner/autos/LT  - OP - SUTO - Climb.auto
diff --git a/src/deploy/pathplanner/autos/LT - BPTL - SUTO - DP.auto b/src/main/deploy/pathplanner/autos/LT - BPTL - SUTO - DP.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/LT - BPTL - SUTO - DP.auto
rename to src/main/deploy/pathplanner/autos/LT - BPTL - SUTO - DP.auto
diff --git a/src/deploy/pathplanner/autos/LT - DP - SUTO - Climb.auto b/src/main/deploy/pathplanner/autos/LT - DP - SUTO - Climb.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/LT - DP - SUTO - Climb.auto
rename to src/main/deploy/pathplanner/autos/LT - DP - SUTO - Climb.auto
diff --git a/src/deploy/pathplanner/autos/LT - DP - SUTO - OP .auto b/src/main/deploy/pathplanner/autos/LT - DP - SUTO - OP .auto
similarity index 100%
rename from src/deploy/pathplanner/autos/LT - DP - SUTO - OP .auto
rename to src/main/deploy/pathplanner/autos/LT - DP - SUTO - OP .auto
diff --git a/src/deploy/pathplanner/autos/LT - RT - SUTO Bump Auto.auto b/src/main/deploy/pathplanner/autos/LT - RT - SUTO Bump Auto.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/LT - RT - SUTO Bump Auto.auto
rename to src/main/deploy/pathplanner/autos/LT - RT - SUTO Bump Auto.auto
diff --git a/src/deploy/pathplanner/autos/LT - RT - SUTO Bump Climb Auto.auto b/src/main/deploy/pathplanner/autos/LT - RT - SUTO Bump Climb Auto.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/LT - RT - SUTO Bump Climb Auto.auto
rename to src/main/deploy/pathplanner/autos/LT - RT - SUTO Bump Climb Auto.auto
diff --git a/src/deploy/pathplanner/autos/LT Locked Auto.auto b/src/main/deploy/pathplanner/autos/LT Locked Auto.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/LT Locked Auto.auto
rename to src/main/deploy/pathplanner/autos/LT Locked Auto.auto
diff --git a/src/deploy/pathplanner/autos/LT Repetitive.auto b/src/main/deploy/pathplanner/autos/LT Repetitive.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/LT Repetitive.auto
rename to src/main/deploy/pathplanner/autos/LT Repetitive.auto
diff --git a/src/deploy/pathplanner/autos/LT Round the World to Climb.auto b/src/main/deploy/pathplanner/autos/LT Round the World to Climb.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/LT Round the World to Climb.auto
rename to src/main/deploy/pathplanner/autos/LT Round the World to Climb.auto
diff --git a/src/deploy/pathplanner/autos/LT Round the World to DP.auto b/src/main/deploy/pathplanner/autos/LT Round the World to DP.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/LT Round the World to DP.auto
rename to src/main/deploy/pathplanner/autos/LT Round the World to DP.auto
diff --git a/src/deploy/pathplanner/autos/LT Round the World to OP.auto b/src/main/deploy/pathplanner/autos/LT Round the World to OP.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/LT Round the World to OP.auto
rename to src/main/deploy/pathplanner/autos/LT Round the World to OP.auto
diff --git a/src/deploy/pathplanner/autos/LT to RT MoveShot.auto b/src/main/deploy/pathplanner/autos/LT to RT MoveShot.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/LT to RT MoveShot.auto
rename to src/main/deploy/pathplanner/autos/LT to RT MoveShot.auto
diff --git a/src/deploy/pathplanner/autos/MS - DP - SUTO - Climb .auto b/src/main/deploy/pathplanner/autos/MS - DP - SUTO - Climb .auto
similarity index 100%
rename from src/deploy/pathplanner/autos/MS - DP - SUTO - Climb .auto
rename to src/main/deploy/pathplanner/autos/MS - DP - SUTO - Climb .auto
diff --git a/src/deploy/pathplanner/autos/MS - DP - SUTO - OP (No Intake).auto b/src/main/deploy/pathplanner/autos/MS - DP - SUTO - OP (No Intake).auto
similarity index 100%
rename from src/deploy/pathplanner/autos/MS - DP - SUTO - OP (No Intake).auto
rename to src/main/deploy/pathplanner/autos/MS - DP - SUTO - OP (No Intake).auto
diff --git a/src/deploy/pathplanner/autos/MS - LT - RT - SUTO Bump Auto.auto b/src/main/deploy/pathplanner/autos/MS - LT - RT - SUTO Bump Auto.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/MS - LT - RT - SUTO Bump Auto.auto
rename to src/main/deploy/pathplanner/autos/MS - LT - RT - SUTO Bump Auto.auto
diff --git a/src/deploy/pathplanner/autos/MS - LT - RT - SUTO Bump Climb Auto.auto b/src/main/deploy/pathplanner/autos/MS - LT - RT - SUTO Bump Climb Auto.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/MS - LT - RT - SUTO Bump Climb Auto.auto
rename to src/main/deploy/pathplanner/autos/MS - LT - RT - SUTO Bump Climb Auto.auto
diff --git a/src/deploy/pathplanner/autos/MS - LT MoveShot.auto b/src/main/deploy/pathplanner/autos/MS - LT MoveShot.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/MS - LT MoveShot.auto
rename to src/main/deploy/pathplanner/autos/MS - LT MoveShot.auto
diff --git a/src/deploy/pathplanner/autos/MS - LT Repetitive.auto b/src/main/deploy/pathplanner/autos/MS - LT Repetitive.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/MS - LT Repetitive.auto
rename to src/main/deploy/pathplanner/autos/MS - LT Repetitive.auto
diff --git a/src/deploy/pathplanner/autos/MS - OP - SUTO - Climb.auto b/src/main/deploy/pathplanner/autos/MS - OP - SUTO - Climb.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/MS - OP - SUTO - Climb.auto
rename to src/main/deploy/pathplanner/autos/MS - OP - SUTO - Climb.auto
diff --git a/src/deploy/pathplanner/autos/MS - RT MoveShot.auto b/src/main/deploy/pathplanner/autos/MS - RT MoveShot.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/MS - RT MoveShot.auto
rename to src/main/deploy/pathplanner/autos/MS - RT MoveShot.auto
diff --git a/src/deploy/pathplanner/autos/MS - RT Repetitive.auto b/src/main/deploy/pathplanner/autos/MS - RT Repetitive.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/MS - RT Repetitive.auto
rename to src/main/deploy/pathplanner/autos/MS - RT Repetitive.auto
diff --git a/src/deploy/pathplanner/autos/MS Round the World to Climb.auto b/src/main/deploy/pathplanner/autos/MS Round the World to Climb.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/MS Round the World to Climb.auto
rename to src/main/deploy/pathplanner/autos/MS Round the World to Climb.auto
diff --git a/src/deploy/pathplanner/autos/MS Round the World to DP.auto b/src/main/deploy/pathplanner/autos/MS Round the World to DP.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/MS Round the World to DP.auto
rename to src/main/deploy/pathplanner/autos/MS Round the World to DP.auto
diff --git a/src/deploy/pathplanner/autos/MS Round the World to OP.auto b/src/main/deploy/pathplanner/autos/MS Round the World to OP.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/MS Round the World to OP.auto
rename to src/main/deploy/pathplanner/autos/MS Round the World to OP.auto
diff --git a/src/deploy/pathplanner/autos/MS-LT Round the World - ASSIST.auto b/src/main/deploy/pathplanner/autos/MS-LT Round the World - ASSIST.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/MS-LT Round the World - ASSIST.auto
rename to src/main/deploy/pathplanner/autos/MS-LT Round the World - ASSIST.auto
diff --git a/src/deploy/pathplanner/autos/MS-LT Round the World - HOARD.auto b/src/main/deploy/pathplanner/autos/MS-LT Round the World - HOARD.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/MS-LT Round the World - HOARD.auto
rename to src/main/deploy/pathplanner/autos/MS-LT Round the World - HOARD.auto
diff --git a/src/deploy/pathplanner/autos/MS-RT Round the World - ASSIST.auto b/src/main/deploy/pathplanner/autos/MS-RT Round the World - ASSIST.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/MS-RT Round the World - ASSIST.auto
rename to src/main/deploy/pathplanner/autos/MS-RT Round the World - ASSIST.auto
diff --git a/src/deploy/pathplanner/autos/MS-RT Round the World - HOAR.auto b/src/main/deploy/pathplanner/autos/MS-RT Round the World - HOAR.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/MS-RT Round the World - HOAR.auto
rename to src/main/deploy/pathplanner/autos/MS-RT Round the World - HOAR.auto
diff --git a/src/deploy/pathplanner/autos/RT - BPBR - SUTO - DP.auto b/src/main/deploy/pathplanner/autos/RT - BPBR - SUTO - DP.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/RT - BPBR - SUTO - DP.auto
rename to src/main/deploy/pathplanner/autos/RT - BPBR - SUTO - DP.auto
diff --git a/src/deploy/pathplanner/autos/RT - BPBR - SUTO - OP.auto b/src/main/deploy/pathplanner/autos/RT - BPBR - SUTO - OP.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/RT - BPBR - SUTO - OP.auto
rename to src/main/deploy/pathplanner/autos/RT - BPBR - SUTO - OP.auto
diff --git a/src/deploy/pathplanner/autos/RT - DP - SUTO - Climb.auto b/src/main/deploy/pathplanner/autos/RT - DP - SUTO - Climb.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/RT - DP - SUTO - Climb.auto
rename to src/main/deploy/pathplanner/autos/RT - DP - SUTO - Climb.auto
diff --git a/src/deploy/pathplanner/autos/RT - DP - SUTO - OP .auto b/src/main/deploy/pathplanner/autos/RT - DP - SUTO - OP .auto
similarity index 100%
rename from src/deploy/pathplanner/autos/RT - DP - SUTO - OP .auto
rename to src/main/deploy/pathplanner/autos/RT - DP - SUTO - OP .auto
diff --git a/src/deploy/pathplanner/autos/RT - LT - SUTO Bump Auto.auto b/src/main/deploy/pathplanner/autos/RT - LT - SUTO Bump Auto.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/RT - LT - SUTO Bump Auto.auto
rename to src/main/deploy/pathplanner/autos/RT - LT - SUTO Bump Auto.auto
diff --git a/src/deploy/pathplanner/autos/RT - LT - SUTO Bump Climb Auto.auto b/src/main/deploy/pathplanner/autos/RT - LT - SUTO Bump Climb Auto.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/RT - LT - SUTO Bump Climb Auto.auto
rename to src/main/deploy/pathplanner/autos/RT - LT - SUTO Bump Climb Auto.auto
diff --git a/src/deploy/pathplanner/autos/RT - LT MoveShot.auto b/src/main/deploy/pathplanner/autos/RT - LT MoveShot.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/RT - LT MoveShot.auto
rename to src/main/deploy/pathplanner/autos/RT - LT MoveShot.auto
diff --git a/src/deploy/pathplanner/autos/RT - OP - SUTO - Climb.auto b/src/main/deploy/pathplanner/autos/RT - OP - SUTO - Climb.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/RT - OP - SUTO - Climb.auto
rename to src/main/deploy/pathplanner/autos/RT - OP - SUTO - Climb.auto
diff --git a/src/deploy/pathplanner/autos/RT Locked Auto.auto b/src/main/deploy/pathplanner/autos/RT Locked Auto.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/RT Locked Auto.auto
rename to src/main/deploy/pathplanner/autos/RT Locked Auto.auto
diff --git a/src/deploy/pathplanner/autos/RT Repetitive.auto b/src/main/deploy/pathplanner/autos/RT Repetitive.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/RT Repetitive.auto
rename to src/main/deploy/pathplanner/autos/RT Repetitive.auto
diff --git a/src/deploy/pathplanner/autos/RT Round the World to Climb.auto b/src/main/deploy/pathplanner/autos/RT Round the World to Climb.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/RT Round the World to Climb.auto
rename to src/main/deploy/pathplanner/autos/RT Round the World to Climb.auto
diff --git a/src/deploy/pathplanner/autos/RT Round the World to DP.auto b/src/main/deploy/pathplanner/autos/RT Round the World to DP.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/RT Round the World to DP.auto
rename to src/main/deploy/pathplanner/autos/RT Round the World to DP.auto
diff --git a/src/deploy/pathplanner/autos/RT Round the World to OP.auto b/src/main/deploy/pathplanner/autos/RT Round the World to OP.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/RT Round the World to OP.auto
rename to src/main/deploy/pathplanner/autos/RT Round the World to OP.auto
diff --git a/src/deploy/pathplanner/autos/URI Center.auto b/src/main/deploy/pathplanner/autos/URI Center.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/URI Center.auto
rename to src/main/deploy/pathplanner/autos/URI Center.auto
diff --git a/src/deploy/pathplanner/autos/URI Left Depot.auto b/src/main/deploy/pathplanner/autos/URI Left Depot.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/URI Left Depot.auto
rename to src/main/deploy/pathplanner/autos/URI Left Depot.auto
diff --git a/src/deploy/pathplanner/autos/URI Right Outpost.auto b/src/main/deploy/pathplanner/autos/URI Right Outpost.auto
similarity index 100%
rename from src/deploy/pathplanner/autos/URI Right Outpost.auto
rename to src/main/deploy/pathplanner/autos/URI Right Outpost.auto
diff --git a/src/deploy/pathplanner/paths/ 8 Point to ACTUALMiddleBallPit.path b/src/main/deploy/pathplanner/paths/ 8 Point to ACTUALMiddleBallPit.path
similarity index 100%
rename from src/deploy/pathplanner/paths/ 8 Point to ACTUALMiddleBallPit.path
rename to src/main/deploy/pathplanner/paths/ 8 Point to ACTUALMiddleBallPit.path
diff --git a/src/deploy/pathplanner/paths/ LT Corner 3 to MidBPTL (2).path b/src/main/deploy/pathplanner/paths/ LT Corner 3 to MidBPTL (2).path
similarity index 100%
rename from src/deploy/pathplanner/paths/ LT Corner 3 to MidBPTL (2).path
rename to src/main/deploy/pathplanner/paths/ LT Corner 3 to MidBPTL (2).path
diff --git a/src/deploy/pathplanner/paths/ MS - RTBump.path b/src/main/deploy/pathplanner/paths/ MS - RTBump.path
similarity index 100%
rename from src/deploy/pathplanner/paths/ MS - RTBump.path
rename to src/main/deploy/pathplanner/paths/ MS - RTBump.path
diff --git a/src/deploy/pathplanner/paths/ RT Corner 3 - MidBPBR (2).path b/src/main/deploy/pathplanner/paths/ RT Corner 3 - MidBPBR (2).path
similarity index 100%
rename from src/deploy/pathplanner/paths/ RT Corner 3 - MidBPBR (2).path
rename to src/main/deploy/pathplanner/paths/ RT Corner 3 - MidBPBR (2).path
diff --git a/src/deploy/pathplanner/paths/8Point to MidBPTL.path b/src/main/deploy/pathplanner/paths/8Point to MidBPTL.path
similarity index 100%
rename from src/deploy/pathplanner/paths/8Point to MidBPTL.path
rename to src/main/deploy/pathplanner/paths/8Point to MidBPTL.path
diff --git a/src/deploy/pathplanner/paths/ACTUALMiddle to RTBump.path b/src/main/deploy/pathplanner/paths/ACTUALMiddle to RTBump.path
similarity index 100%
rename from src/deploy/pathplanner/paths/ACTUALMiddle to RTBump.path
rename to src/main/deploy/pathplanner/paths/ACTUALMiddle to RTBump.path
diff --git a/src/deploy/pathplanner/paths/ACTUALMiddleBallPit to BPTL.path b/src/main/deploy/pathplanner/paths/ACTUALMiddleBallPit to BPTL.path
similarity index 100%
rename from src/deploy/pathplanner/paths/ACTUALMiddleBallPit to BPTL.path
rename to src/main/deploy/pathplanner/paths/ACTUALMiddleBallPit to BPTL.path
diff --git a/src/deploy/pathplanner/paths/ACTUALMiddleBallPit to LT.path b/src/main/deploy/pathplanner/paths/ACTUALMiddleBallPit to LT.path
similarity index 100%
rename from src/deploy/pathplanner/paths/ACTUALMiddleBallPit to LT.path
rename to src/main/deploy/pathplanner/paths/ACTUALMiddleBallPit to LT.path
diff --git a/src/deploy/pathplanner/paths/ACTUALMiddleBallPit to LTBump.path b/src/main/deploy/pathplanner/paths/ACTUALMiddleBallPit to LTBump.path
similarity index 100%
rename from src/deploy/pathplanner/paths/ACTUALMiddleBallPit to LTBump.path
rename to src/main/deploy/pathplanner/paths/ACTUALMiddleBallPit to LTBump.path
diff --git a/src/deploy/pathplanner/paths/ACTUALMiddleBallPit to RT.path b/src/main/deploy/pathplanner/paths/ACTUALMiddleBallPit to RT.path
similarity index 100%
rename from src/deploy/pathplanner/paths/ACTUALMiddleBallPit to RT.path
rename to src/main/deploy/pathplanner/paths/ACTUALMiddleBallPit to RT.path
diff --git a/src/deploy/pathplanner/paths/ACTUALMiddleBallPit2 to BPBR.path b/src/main/deploy/pathplanner/paths/ACTUALMiddleBallPit2 to BPBR.path
similarity index 100%
rename from src/deploy/pathplanner/paths/ACTUALMiddleBallPit2 to BPBR.path
rename to src/main/deploy/pathplanner/paths/ACTUALMiddleBallPit2 to BPBR.path
diff --git a/src/deploy/pathplanner/paths/BPBR - RTBump.path b/src/main/deploy/pathplanner/paths/BPBR - RTBump.path
similarity index 100%
rename from src/deploy/pathplanner/paths/BPBR - RTBump.path
rename to src/main/deploy/pathplanner/paths/BPBR - RTBump.path
diff --git a/src/deploy/pathplanner/paths/BPBR to MiddleBallPit.path b/src/main/deploy/pathplanner/paths/BPBR to MiddleBallPit.path
similarity index 100%
rename from src/deploy/pathplanner/paths/BPBR to MiddleBallPit.path
rename to src/main/deploy/pathplanner/paths/BPBR to MiddleBallPit.path
diff --git a/src/deploy/pathplanner/paths/BPBR to RT.path b/src/main/deploy/pathplanner/paths/BPBR to RT.path
similarity index 100%
rename from src/deploy/pathplanner/paths/BPBR to RT.path
rename to src/main/deploy/pathplanner/paths/BPBR to RT.path
diff --git a/src/deploy/pathplanner/paths/BPBR to RTBump.path b/src/main/deploy/pathplanner/paths/BPBR to RTBump.path
similarity index 100%
rename from src/deploy/pathplanner/paths/BPBR to RTBump.path
rename to src/main/deploy/pathplanner/paths/BPBR to RTBump.path
diff --git a/src/deploy/pathplanner/paths/BPBR to RTCorner3.path b/src/main/deploy/pathplanner/paths/BPBR to RTCorner3.path
similarity index 100%
rename from src/deploy/pathplanner/paths/BPBR to RTCorner3.path
rename to src/main/deploy/pathplanner/paths/BPBR to RTCorner3.path
diff --git a/src/deploy/pathplanner/paths/BPTL to ACTUALMiddleversion2.path b/src/main/deploy/pathplanner/paths/BPTL to ACTUALMiddleversion2.path
similarity index 100%
rename from src/deploy/pathplanner/paths/BPTL to ACTUALMiddleversion2.path
rename to src/main/deploy/pathplanner/paths/BPTL to ACTUALMiddleversion2.path
diff --git a/src/deploy/pathplanner/paths/BPTL to LT.path b/src/main/deploy/pathplanner/paths/BPTL to LT.path
similarity index 100%
rename from src/deploy/pathplanner/paths/BPTL to LT.path
rename to src/main/deploy/pathplanner/paths/BPTL to LT.path
diff --git a/src/deploy/pathplanner/paths/Bottom to Rotated Top.path b/src/main/deploy/pathplanner/paths/Bottom to Rotated Top.path
similarity index 100%
rename from src/deploy/pathplanner/paths/Bottom to Rotated Top.path
rename to src/main/deploy/pathplanner/paths/Bottom to Rotated Top.path
diff --git a/src/deploy/pathplanner/paths/Bottom to Top.path b/src/main/deploy/pathplanner/paths/Bottom to Top.path
similarity index 100%
rename from src/deploy/pathplanner/paths/Bottom to Top.path
rename to src/main/deploy/pathplanner/paths/Bottom to Top.path
diff --git a/src/deploy/pathplanner/paths/C to C Tower.path b/src/main/deploy/pathplanner/paths/C to C Tower.path
similarity index 100%
rename from src/deploy/pathplanner/paths/C to C Tower.path
rename to src/main/deploy/pathplanner/paths/C to C Tower.path
diff --git a/src/deploy/pathplanner/paths/CornerLine LT to DP.path b/src/main/deploy/pathplanner/paths/CornerLine LT to DP.path
similarity index 100%
rename from src/deploy/pathplanner/paths/CornerLine LT to DP.path
rename to src/main/deploy/pathplanner/paths/CornerLine LT to DP.path
diff --git a/src/deploy/pathplanner/paths/DP to Climb.path b/src/main/deploy/pathplanner/paths/DP to Climb.path
similarity index 100%
rename from src/deploy/pathplanner/paths/DP to Climb.path
rename to src/main/deploy/pathplanner/paths/DP to Climb.path
diff --git a/src/deploy/pathplanner/paths/DP to SUTO.path b/src/main/deploy/pathplanner/paths/DP to SUTO.path
similarity index 100%
rename from src/deploy/pathplanner/paths/DP to SUTO.path
rename to src/main/deploy/pathplanner/paths/DP to SUTO.path
diff --git a/src/deploy/pathplanner/paths/Depot to Left Tower.path b/src/main/deploy/pathplanner/paths/Depot to Left Tower.path
similarity index 100%
rename from src/deploy/pathplanner/paths/Depot to Left Tower.path
rename to src/main/deploy/pathplanner/paths/Depot to Left Tower.path
diff --git a/src/deploy/pathplanner/paths/Fadeaway Top to Bottom.path b/src/main/deploy/pathplanner/paths/Fadeaway Top to Bottom.path
similarity index 100%
rename from src/deploy/pathplanner/paths/Fadeaway Top to Bottom.path
rename to src/main/deploy/pathplanner/paths/Fadeaway Top to Bottom.path
diff --git a/src/deploy/pathplanner/paths/L Trench to Depot.path b/src/main/deploy/pathplanner/paths/L Trench to Depot.path
similarity index 100%
rename from src/deploy/pathplanner/paths/L Trench to Depot.path
rename to src/main/deploy/pathplanner/paths/L Trench to Depot.path
diff --git a/src/deploy/pathplanner/paths/LT - MidBPTL.path b/src/main/deploy/pathplanner/paths/LT - MidBPTL.path
similarity index 100%
rename from src/deploy/pathplanner/paths/LT - MidBPTL.path
rename to src/main/deploy/pathplanner/paths/LT - MidBPTL.path
diff --git a/src/deploy/pathplanner/paths/LT Corner 3 to 8Point.path b/src/main/deploy/pathplanner/paths/LT Corner 3 to 8Point.path
similarity index 100%
rename from src/deploy/pathplanner/paths/LT Corner 3 to 8Point.path
rename to src/main/deploy/pathplanner/paths/LT Corner 3 to 8Point.path
diff --git a/src/deploy/pathplanner/paths/LT Corner 3 to DP.path b/src/main/deploy/pathplanner/paths/LT Corner 3 to DP.path
similarity index 100%
rename from src/deploy/pathplanner/paths/LT Corner 3 to DP.path
rename to src/main/deploy/pathplanner/paths/LT Corner 3 to DP.path
diff --git a/src/deploy/pathplanner/paths/LT Corner 3 to MidBPTL (Sped Up).path b/src/main/deploy/pathplanner/paths/LT Corner 3 to MidBPTL (Sped Up).path
similarity index 100%
rename from src/deploy/pathplanner/paths/LT Corner 3 to MidBPTL (Sped Up).path
rename to src/main/deploy/pathplanner/paths/LT Corner 3 to MidBPTL (Sped Up).path
diff --git a/src/deploy/pathplanner/paths/LT Corner 3 to MidBPTL.path b/src/main/deploy/pathplanner/paths/LT Corner 3 to MidBPTL.path
similarity index 100%
rename from src/deploy/pathplanner/paths/LT Corner 3 to MidBPTL.path
rename to src/main/deploy/pathplanner/paths/LT Corner 3 to MidBPTL.path
diff --git a/src/deploy/pathplanner/paths/LT Corner 3 to SUTO.path b/src/main/deploy/pathplanner/paths/LT Corner 3 to SUTO.path
similarity index 100%
rename from src/deploy/pathplanner/paths/LT Corner 3 to SUTO.path
rename to src/main/deploy/pathplanner/paths/LT Corner 3 to SUTO.path
diff --git a/src/deploy/pathplanner/paths/LT CornerLine to SUTO.path b/src/main/deploy/pathplanner/paths/LT CornerLine to SUTO.path
similarity index 100%
rename from src/deploy/pathplanner/paths/LT CornerLine to SUTO.path
rename to src/main/deploy/pathplanner/paths/LT CornerLine to SUTO.path
diff --git a/src/deploy/pathplanner/paths/LT To PeakSUTO.path b/src/main/deploy/pathplanner/paths/LT To PeakSUTO.path
similarity index 100%
rename from src/deploy/pathplanner/paths/LT To PeakSUTO.path
rename to src/main/deploy/pathplanner/paths/LT To PeakSUTO.path
diff --git a/src/deploy/pathplanner/paths/LT to BPTL.path b/src/main/deploy/pathplanner/paths/LT to BPTL.path
similarity index 100%
rename from src/deploy/pathplanner/paths/LT to BPTL.path
rename to src/main/deploy/pathplanner/paths/LT to BPTL.path
diff --git a/src/deploy/pathplanner/paths/LT to Climb.path b/src/main/deploy/pathplanner/paths/LT to Climb.path
similarity index 100%
rename from src/deploy/pathplanner/paths/LT to Climb.path
rename to src/main/deploy/pathplanner/paths/LT to Climb.path
diff --git a/src/deploy/pathplanner/paths/LT to DP.path b/src/main/deploy/pathplanner/paths/LT to DP.path
similarity index 100%
rename from src/deploy/pathplanner/paths/LT to DP.path
rename to src/main/deploy/pathplanner/paths/LT to DP.path
diff --git a/src/deploy/pathplanner/paths/LT to LTBump26.path b/src/main/deploy/pathplanner/paths/LT to LTBump26.path
similarity index 100%
rename from src/deploy/pathplanner/paths/LT to LTBump26.path
rename to src/main/deploy/pathplanner/paths/LT to LTBump26.path
diff --git a/src/deploy/pathplanner/paths/LT to OP.path b/src/main/deploy/pathplanner/paths/LT to OP.path
similarity index 100%
rename from src/deploy/pathplanner/paths/LT to OP.path
rename to src/main/deploy/pathplanner/paths/LT to OP.path
diff --git a/src/deploy/pathplanner/paths/LT to Rotated BPTL.path b/src/main/deploy/pathplanner/paths/LT to Rotated BPTL.path
similarity index 100%
rename from src/deploy/pathplanner/paths/LT to Rotated BPTL.path
rename to src/main/deploy/pathplanner/paths/LT to Rotated BPTL.path
diff --git a/src/deploy/pathplanner/paths/LT to Shoot.path b/src/main/deploy/pathplanner/paths/LT to Shoot.path
similarity index 100%
rename from src/deploy/pathplanner/paths/LT to Shoot.path
rename to src/main/deploy/pathplanner/paths/LT to Shoot.path
diff --git a/src/deploy/pathplanner/paths/LTBump - BPTL.path b/src/main/deploy/pathplanner/paths/LTBump - BPTL.path
similarity index 100%
rename from src/deploy/pathplanner/paths/LTBump - BPTL.path
rename to src/main/deploy/pathplanner/paths/LTBump - BPTL.path
diff --git a/src/deploy/pathplanner/paths/LTBump to DP.path b/src/main/deploy/pathplanner/paths/LTBump to DP.path
similarity index 100%
rename from src/deploy/pathplanner/paths/LTBump to DP.path
rename to src/main/deploy/pathplanner/paths/LTBump to DP.path
diff --git a/src/deploy/pathplanner/paths/LTBump to LTCorner 3.path b/src/main/deploy/pathplanner/paths/LTBump to LTCorner 3.path
similarity index 100%
rename from src/deploy/pathplanner/paths/LTBump to LTCorner 3.path
rename to src/main/deploy/pathplanner/paths/LTBump to LTCorner 3.path
diff --git a/src/deploy/pathplanner/paths/LTBump to SUTO.path b/src/main/deploy/pathplanner/paths/LTBump to SUTO.path
similarity index 100%
rename from src/deploy/pathplanner/paths/LTBump to SUTO.path
rename to src/main/deploy/pathplanner/paths/LTBump to SUTO.path
diff --git a/src/deploy/pathplanner/paths/MS - LT Corner 3.path b/src/main/deploy/pathplanner/paths/MS - LT Corner 3.path
similarity index 100%
rename from src/deploy/pathplanner/paths/MS - LT Corner 3.path
rename to src/main/deploy/pathplanner/paths/MS - LT Corner 3.path
diff --git a/src/deploy/pathplanner/paths/MS - LTBump.path b/src/main/deploy/pathplanner/paths/MS - LTBump.path
similarity index 100%
rename from src/deploy/pathplanner/paths/MS - LTBump.path
rename to src/main/deploy/pathplanner/paths/MS - LTBump.path
diff --git a/src/deploy/pathplanner/paths/MS - RT Corner 3.path b/src/main/deploy/pathplanner/paths/MS - RT Corner 3.path
similarity index 100%
rename from src/deploy/pathplanner/paths/MS - RT Corner 3.path
rename to src/main/deploy/pathplanner/paths/MS - RT Corner 3.path
diff --git a/src/deploy/pathplanner/paths/MS to Climb.path b/src/main/deploy/pathplanner/paths/MS to Climb.path
similarity index 100%
rename from src/deploy/pathplanner/paths/MS to Climb.path
rename to src/main/deploy/pathplanner/paths/MS to Climb.path
diff --git a/src/deploy/pathplanner/paths/MS to DP.path b/src/main/deploy/pathplanner/paths/MS to DP.path
similarity index 100%
rename from src/deploy/pathplanner/paths/MS to DP.path
rename to src/main/deploy/pathplanner/paths/MS to DP.path
diff --git a/src/deploy/pathplanner/paths/MS to LT.path b/src/main/deploy/pathplanner/paths/MS to LT.path
similarity index 100%
rename from src/deploy/pathplanner/paths/MS to LT.path
rename to src/main/deploy/pathplanner/paths/MS to LT.path
diff --git a/src/deploy/pathplanner/paths/MS to OP.path b/src/main/deploy/pathplanner/paths/MS to OP.path
similarity index 100%
rename from src/deploy/pathplanner/paths/MS to OP.path
rename to src/main/deploy/pathplanner/paths/MS to OP.path
diff --git a/src/deploy/pathplanner/paths/MS to RT.path b/src/main/deploy/pathplanner/paths/MS to RT.path
similarity index 100%
rename from src/deploy/pathplanner/paths/MS to RT.path
rename to src/main/deploy/pathplanner/paths/MS to RT.path
diff --git a/src/deploy/pathplanner/paths/MidBPBR - RT Corner 3.path b/src/main/deploy/pathplanner/paths/MidBPBR - RT Corner 3.path
similarity index 100%
rename from src/deploy/pathplanner/paths/MidBPBR - RT Corner 3.path
rename to src/main/deploy/pathplanner/paths/MidBPBR - RT Corner 3.path
diff --git a/src/deploy/pathplanner/paths/MidBPTL - LT Corner 3.path b/src/main/deploy/pathplanner/paths/MidBPTL - LT Corner 3.path
similarity index 100%
rename from src/deploy/pathplanner/paths/MidBPTL - LT Corner 3.path
rename to src/main/deploy/pathplanner/paths/MidBPTL - LT Corner 3.path
diff --git a/src/deploy/pathplanner/paths/OP to SUTO.path b/src/main/deploy/pathplanner/paths/OP to SUTO.path
similarity index 100%
rename from src/deploy/pathplanner/paths/OP to SUTO.path
rename to src/main/deploy/pathplanner/paths/OP to SUTO.path
diff --git a/src/deploy/pathplanner/paths/Outpost Int Ext to Right Tower.path b/src/main/deploy/pathplanner/paths/Outpost Int Ext to Right Tower.path
similarity index 100%
rename from src/deploy/pathplanner/paths/Outpost Int Ext to Right Tower.path
rename to src/main/deploy/pathplanner/paths/Outpost Int Ext to Right Tower.path
diff --git a/src/deploy/pathplanner/paths/R Trench to Depot Intake Ext.path b/src/main/deploy/pathplanner/paths/R Trench to Depot Intake Ext.path
similarity index 100%
rename from src/deploy/pathplanner/paths/R Trench to Depot Intake Ext.path
rename to src/main/deploy/pathplanner/paths/R Trench to Depot Intake Ext.path
diff --git a/src/deploy/pathplanner/paths/RT - MidBPBR.path b/src/main/deploy/pathplanner/paths/RT - MidBPBR.path
similarity index 100%
rename from src/deploy/pathplanner/paths/RT - MidBPBR.path
rename to src/main/deploy/pathplanner/paths/RT - MidBPBR.path
diff --git a/src/deploy/pathplanner/paths/RT - PeakSUTO.path b/src/main/deploy/pathplanner/paths/RT - PeakSUTO.path
similarity index 100%
rename from src/deploy/pathplanner/paths/RT - PeakSUTO.path
rename to src/main/deploy/pathplanner/paths/RT - PeakSUTO.path
diff --git a/src/deploy/pathplanner/paths/RT 8 Point to ACTUALMiddleBallPit2.path b/src/main/deploy/pathplanner/paths/RT 8 Point to ACTUALMiddleBallPit2.path
similarity index 100%
rename from src/deploy/pathplanner/paths/RT 8 Point to ACTUALMiddleBallPit2.path
rename to src/main/deploy/pathplanner/paths/RT 8 Point to ACTUALMiddleBallPit2.path
diff --git a/src/deploy/pathplanner/paths/RT Corner 3 - MidBPBR (2) Sped Up.path b/src/main/deploy/pathplanner/paths/RT Corner 3 - MidBPBR (2) Sped Up.path
similarity index 100%
rename from src/deploy/pathplanner/paths/RT Corner 3 - MidBPBR (2) Sped Up.path
rename to src/main/deploy/pathplanner/paths/RT Corner 3 - MidBPBR (2) Sped Up.path
diff --git a/src/deploy/pathplanner/paths/RT Corner 3 - MidBPBR (Sped Up).path b/src/main/deploy/pathplanner/paths/RT Corner 3 - MidBPBR (Sped Up).path
similarity index 100%
rename from src/deploy/pathplanner/paths/RT Corner 3 - MidBPBR (Sped Up).path
rename to src/main/deploy/pathplanner/paths/RT Corner 3 - MidBPBR (Sped Up).path
diff --git a/src/deploy/pathplanner/paths/RT Corner 3 - MidBPBR.path b/src/main/deploy/pathplanner/paths/RT Corner 3 - MidBPBR.path
similarity index 100%
rename from src/deploy/pathplanner/paths/RT Corner 3 - MidBPBR.path
rename to src/main/deploy/pathplanner/paths/RT Corner 3 - MidBPBR.path
diff --git a/src/deploy/pathplanner/paths/RT Corner 3 ro RT 8 Point.path b/src/main/deploy/pathplanner/paths/RT Corner 3 ro RT 8 Point.path
similarity index 100%
rename from src/deploy/pathplanner/paths/RT Corner 3 ro RT 8 Point.path
rename to src/main/deploy/pathplanner/paths/RT Corner 3 ro RT 8 Point.path
diff --git a/src/deploy/pathplanner/paths/RT Corner 3 to OP.path b/src/main/deploy/pathplanner/paths/RT Corner 3 to OP.path
similarity index 100%
rename from src/deploy/pathplanner/paths/RT Corner 3 to OP.path
rename to src/main/deploy/pathplanner/paths/RT Corner 3 to OP.path
diff --git a/src/deploy/pathplanner/paths/RT Corner3 to SUTO.path b/src/main/deploy/pathplanner/paths/RT Corner3 to SUTO.path
similarity index 100%
rename from src/deploy/pathplanner/paths/RT Corner3 to SUTO.path
rename to src/main/deploy/pathplanner/paths/RT Corner3 to SUTO.path
diff --git a/src/deploy/pathplanner/paths/RT Corner3-DP.path b/src/main/deploy/pathplanner/paths/RT Corner3-DP.path
similarity index 100%
rename from src/deploy/pathplanner/paths/RT Corner3-DP.path
rename to src/main/deploy/pathplanner/paths/RT Corner3-DP.path
diff --git a/src/deploy/pathplanner/paths/RT to Climb.path b/src/main/deploy/pathplanner/paths/RT to Climb.path
similarity index 100%
rename from src/deploy/pathplanner/paths/RT to Climb.path
rename to src/main/deploy/pathplanner/paths/RT to Climb.path
diff --git a/src/deploy/pathplanner/paths/RT to DP.path b/src/main/deploy/pathplanner/paths/RT to DP.path
similarity index 100%
rename from src/deploy/pathplanner/paths/RT to DP.path
rename to src/main/deploy/pathplanner/paths/RT to DP.path
diff --git a/src/deploy/pathplanner/paths/RT to OP.path b/src/main/deploy/pathplanner/paths/RT to OP.path
similarity index 100%
rename from src/deploy/pathplanner/paths/RT to OP.path
rename to src/main/deploy/pathplanner/paths/RT to OP.path
diff --git a/src/deploy/pathplanner/paths/RT to RTBump26.path b/src/main/deploy/pathplanner/paths/RT to RTBump26.path
similarity index 100%
rename from src/deploy/pathplanner/paths/RT to RTBump26.path
rename to src/main/deploy/pathplanner/paths/RT to RTBump26.path
diff --git a/src/deploy/pathplanner/paths/RT to Rotated BPBR.path b/src/main/deploy/pathplanner/paths/RT to Rotated BPBR.path
similarity index 100%
rename from src/deploy/pathplanner/paths/RT to Rotated BPBR.path
rename to src/main/deploy/pathplanner/paths/RT to Rotated BPBR.path
diff --git a/src/deploy/pathplanner/paths/RT to Shoot.path b/src/main/deploy/pathplanner/paths/RT to Shoot.path
similarity index 100%
rename from src/deploy/pathplanner/paths/RT to Shoot.path
rename to src/main/deploy/pathplanner/paths/RT to Shoot.path
diff --git a/src/deploy/pathplanner/paths/RTBump - BPBR.path b/src/main/deploy/pathplanner/paths/RTBump - BPBR.path
similarity index 100%
rename from src/deploy/pathplanner/paths/RTBump - BPBR.path
rename to src/main/deploy/pathplanner/paths/RTBump - BPBR.path
diff --git a/src/deploy/pathplanner/paths/RTBump to OP.path b/src/main/deploy/pathplanner/paths/RTBump to OP.path
similarity index 100%
rename from src/deploy/pathplanner/paths/RTBump to OP.path
rename to src/main/deploy/pathplanner/paths/RTBump to OP.path
diff --git a/src/deploy/pathplanner/paths/RTBump to RTCorner 3.path b/src/main/deploy/pathplanner/paths/RTBump to RTCorner 3.path
similarity index 100%
rename from src/deploy/pathplanner/paths/RTBump to RTCorner 3.path
rename to src/main/deploy/pathplanner/paths/RTBump to RTCorner 3.path
diff --git a/src/deploy/pathplanner/paths/RTBump to SUTO.path b/src/main/deploy/pathplanner/paths/RTBump to SUTO.path
similarity index 100%
rename from src/deploy/pathplanner/paths/RTBump to SUTO.path
rename to src/main/deploy/pathplanner/paths/RTBump to SUTO.path
diff --git a/src/deploy/pathplanner/paths/Rotated BPBR to RT Corner 3.path b/src/main/deploy/pathplanner/paths/Rotated BPBR to RT Corner 3.path
similarity index 100%
rename from src/deploy/pathplanner/paths/Rotated BPBR to RT Corner 3.path
rename to src/main/deploy/pathplanner/paths/Rotated BPBR to RT Corner 3.path
diff --git a/src/deploy/pathplanner/paths/Rotated BPBR to RT.path b/src/main/deploy/pathplanner/paths/Rotated BPBR to RT.path
similarity index 100%
rename from src/deploy/pathplanner/paths/Rotated BPBR to RT.path
rename to src/main/deploy/pathplanner/paths/Rotated BPBR to RT.path
diff --git a/src/deploy/pathplanner/paths/Rotated BPTL - LTBump.path b/src/main/deploy/pathplanner/paths/Rotated BPTL - LTBump.path
similarity index 100%
rename from src/deploy/pathplanner/paths/Rotated BPTL - LTBump.path
rename to src/main/deploy/pathplanner/paths/Rotated BPTL - LTBump.path
diff --git a/src/deploy/pathplanner/paths/Rotated BPTL to LT Corrner 3.path b/src/main/deploy/pathplanner/paths/Rotated BPTL to LT Corrner 3.path
similarity index 100%
rename from src/deploy/pathplanner/paths/Rotated BPTL to LT Corrner 3.path
rename to src/main/deploy/pathplanner/paths/Rotated BPTL to LT Corrner 3.path
diff --git a/src/deploy/pathplanner/paths/Rotated BPTL to LT.path b/src/main/deploy/pathplanner/paths/Rotated BPTL to LT.path
similarity index 100%
rename from src/deploy/pathplanner/paths/Rotated BPTL to LT.path
rename to src/main/deploy/pathplanner/paths/Rotated BPTL to LT.path
diff --git a/src/deploy/pathplanner/paths/RotatedBPBR to RT Corner 3.path b/src/main/deploy/pathplanner/paths/RotatedBPBR to RT Corner 3.path
similarity index 100%
rename from src/deploy/pathplanner/paths/RotatedBPBR to RT Corner 3.path
rename to src/main/deploy/pathplanner/paths/RotatedBPBR to RT Corner 3.path
diff --git a/src/deploy/pathplanner/paths/RotatedBPTL to LT Corner 3.path b/src/main/deploy/pathplanner/paths/RotatedBPTL to LT Corner 3.path
similarity index 100%
rename from src/deploy/pathplanner/paths/RotatedBPTL to LT Corner 3.path
rename to src/main/deploy/pathplanner/paths/RotatedBPTL to LT Corner 3.path
diff --git a/src/deploy/pathplanner/paths/SUTO - OP.path b/src/main/deploy/pathplanner/paths/SUTO - OP.path
similarity index 100%
rename from src/deploy/pathplanner/paths/SUTO - OP.path
rename to src/main/deploy/pathplanner/paths/SUTO - OP.path
diff --git a/src/deploy/pathplanner/paths/SUTO to Climb.path b/src/main/deploy/pathplanner/paths/SUTO to Climb.path
similarity index 100%
rename from src/deploy/pathplanner/paths/SUTO to Climb.path
rename to src/main/deploy/pathplanner/paths/SUTO to Climb.path
diff --git a/src/deploy/pathplanner/paths/SUTO to DP.path b/src/main/deploy/pathplanner/paths/SUTO to DP.path
similarity index 100%
rename from src/deploy/pathplanner/paths/SUTO to DP.path
rename to src/main/deploy/pathplanner/paths/SUTO to DP.path
diff --git a/src/deploy/pathplanner/paths/SUTO to LT.path b/src/main/deploy/pathplanner/paths/SUTO to LT.path
similarity index 100%
rename from src/deploy/pathplanner/paths/SUTO to LT.path
rename to src/main/deploy/pathplanner/paths/SUTO to LT.path
diff --git a/src/deploy/pathplanner/paths/SUTO to RT.path b/src/main/deploy/pathplanner/paths/SUTO to RT.path
similarity index 100%
rename from src/deploy/pathplanner/paths/SUTO to RT.path
rename to src/main/deploy/pathplanner/paths/SUTO to RT.path
diff --git a/src/deploy/pathplanner/paths/Top to Bottom.path b/src/main/deploy/pathplanner/paths/Top to Bottom.path
similarity index 100%
rename from src/deploy/pathplanner/paths/Top to Bottom.path
rename to src/main/deploy/pathplanner/paths/Top to Bottom.path
diff --git a/src/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
similarity index 100%
rename from src/deploy/pathplanner/settings.json
rename to src/main/deploy/pathplanner/settings.json

From e6000e5dbd45f8b4261528e8545b4e5ae340f767 Mon Sep 17 00:00:00 2001
From: Maxwell Morgan 
Date: Fri, 20 Mar 2026 11:20:51 -0400
Subject: [PATCH 60/61] Add comp changes

---
 src/deploy/pathplanner/navgrid.json           | 1576 ++++++++++++++++-
 src/deploy/pathplanner/settings.json          |    2 +-
 src/main/deploy/pathplanner/navgrid.json      | 1576 ++++++++++++++++-
 src/main/java/frc/robot/RobotContainer.java   |  139 +-
 .../frc/robot/control/DefaultControls.java    |   20 +-
 .../frc/robot/control/DriverControls.java     |   22 +-
 .../frc/robot/subsystems/indexer/Indexer.java |    8 +-
 .../subsystems/indexer/IndexerConstants.java  |    2 +-
 .../robot/subsystems/leds/LedConstants.java   |    6 +-
 .../java/frc/robot/subsystems/leds/Leds.java  |   13 +-
 .../frc/robot/subsystems/shooter/Shooter.java |   34 +-
 .../subsystems/shooter/ShooterConstants.java  |    4 +-
 .../shooter/TrajectoryCalculator.java         |    9 +-
 .../shooter/hood/HoodIOSparkMax.java          |    4 +-
 .../shooter/turret/TurretIOSparkMax.java      |    2 +-
 15 files changed, 3315 insertions(+), 102 deletions(-)

diff --git a/src/deploy/pathplanner/navgrid.json b/src/deploy/pathplanner/navgrid.json
index ac5f521..660ca52 100644
--- a/src/deploy/pathplanner/navgrid.json
+++ b/src/deploy/pathplanner/navgrid.json
@@ -1 +1,1575 @@
-{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
+{
+  "field_size": {
+    "x": 16.54,
+    "y": 8.07
+  },
+  "nodeSizeMeters": 0.3,
+  "grid": [
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ]
+  ]
+}
diff --git a/src/deploy/pathplanner/settings.json b/src/deploy/pathplanner/settings.json
index 7cdd769..0cce017 100644
--- a/src/deploy/pathplanner/settings.json
+++ b/src/deploy/pathplanner/settings.json
@@ -34,4 +34,4 @@
   "bumperOffsetX": 0.0,
   "bumperOffsetY": 0.0,
   "robotFeatures": []
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
index ac5f521..660ca52 100644
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -1 +1,1575 @@
-{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
+{
+  "field_size": {
+    "x": 16.54,
+    "y": 8.07
+  },
+  "nodeSizeMeters": 0.3,
+  "grid": [
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ],
+    [
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true,
+      true
+    ]
+  ]
+}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index a3709da..783788b 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,6 +4,9 @@
 
 package frc.robot;
 
+import static edu.wpi.first.units.Units.Seconds;
+
+import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -15,7 +18,6 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import frc.robot.Constants.Mode;
 import frc.robot.RobotState.OdometryObservation;
 import frc.robot.RobotState.VisionMeasurement;
 import frc.robot.control.Configurable;
@@ -34,6 +36,7 @@
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.intake.IntakeIOSim;
 import frc.robot.subsystems.intake.IntakeIOTalonFX;
+import frc.robot.subsystems.leds.Leds;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.flywheel.FlywheelIOSim;
 import frc.robot.subsystems.shooter.flywheel.FlywheelIOTalonFX;
@@ -45,17 +48,9 @@
 import frc.robot.subsystems.vision.Vision;
 import frc.robot.subsystems.vision.Vision.VisionConsumer;
 import frc.robot.util.GeomUtil;
-
-import static edu.wpi.first.units.Units.Seconds;
-
 import java.util.List;
 import java.util.function.Supplier;
 
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.auto.NamedCommands;
-import com.pathplanner.lib.commands.PathPlannerAuto;
-import com.pathplanner.lib.path.PathPlannerPath;
-
 public class RobotContainer {
   private final DriverController driver = new DriverController.XboxDriverController(0);
   private final DriverController operator = new DriverController.XboxDriverController(1);
@@ -64,6 +59,7 @@ public class RobotContainer {
   private Indexer indexer;
   private Intake intake;
   private Shooter shooter;
+  private Leds leds;
   private Vision vision;
 
   private static Field2d field2d = new Field2d();
@@ -80,40 +76,44 @@ public RobotContainer() {
     field2d.setRobotPose(RobotState.getInstance().getEstimatedPose());
     switch (Constants.kCurrentMode) {
       case REAL -> {
-        drive = new Drive(
-            new GyroIOPigeon2(),
-            new ModuleIOTalonFX(TunerConstants.FrontLeft),
-            new ModuleIOTalonFX(TunerConstants.FrontRight),
-            new ModuleIOTalonFX(TunerConstants.BackLeft),
-            new ModuleIOTalonFX(TunerConstants.BackRight));
+        drive =
+            new Drive(
+                new GyroIOPigeon2(),
+                new ModuleIOTalonFX(TunerConstants.FrontLeft),
+                new ModuleIOTalonFX(TunerConstants.FrontRight),
+                new ModuleIOTalonFX(TunerConstants.BackLeft),
+                new ModuleIOTalonFX(TunerConstants.BackRight));
         indexer = new Indexer(new IndexerIOTalonFX());
         intake = new Intake(new IntakeIOTalonFX());
-        shooter = new Shooter(new TurretIOSparkMax() {
-        }, new HoodIOSparkMax(), new FlywheelIOTalonFX());
-        vision = new Vision(
-            new VisionConsumer() {
-              public void accept(
-                  Pose2d visionRobotPoseMeters,
-                  double timestampSeconds,
-                  edu.wpi.first.math.Matrix visionMeasurementStdDevs) {
-
-                RobotState.getInstance()
-                    .addVisionMeasurement(
-                        new VisionMeasurement(
-                            timestampSeconds, visionRobotPoseMeters, visionMeasurementStdDevs));
-              };
-            },
-            new CameraIOLimelight("limelight-front", robotRotationSupplier),
-            new CameraIOLimelight("limelight-one", robotRotationSupplier));
+        leds = Leds.getInstance();
+        shooter =
+            new Shooter(new TurretIOSparkMax() {}, new HoodIOSparkMax(), new FlywheelIOTalonFX());
+        vision =
+            new Vision(
+                new VisionConsumer() {
+                  public void accept(
+                      Pose2d visionRobotPoseMeters,
+                      double timestampSeconds,
+                      edu.wpi.first.math.Matrix visionMeasurementStdDevs) {
+
+                    RobotState.getInstance()
+                        .addVisionMeasurement(
+                            new VisionMeasurement(
+                                timestampSeconds, visionRobotPoseMeters, visionMeasurementStdDevs));
+                  }
+                  ;
+                },
+                new CameraIOLimelight("limelight-front", robotRotationSupplier),
+                new CameraIOLimelight("limelight-one", robotRotationSupplier));
       }
       case SIM -> {
-        drive = new Drive(
-            new GyroIO() {
-            },
-            new ModuleIOSim(TunerConstants.FrontLeft),
-            new ModuleIOSim(TunerConstants.FrontRight),
-            new ModuleIOSim(TunerConstants.BackLeft),
-            new ModuleIOSim(TunerConstants.BackRight));
+        drive =
+            new Drive(
+                new GyroIO() {},
+                new ModuleIOSim(TunerConstants.FrontLeft),
+                new ModuleIOSim(TunerConstants.FrontRight),
+                new ModuleIOSim(TunerConstants.BackLeft),
+                new ModuleIOSim(TunerConstants.BackRight));
         indexer = new Indexer(new IndexerIOSim());
         intake = new Intake(new IntakeIOSim());
         shooter = new Shooter(new TurretIOSim(), new HoodIOSim(), new FlywheelIOSim());
@@ -122,19 +122,19 @@ public void accept(
 
     configureBindings();
 
-    if (Constants.kCurrentMode == Mode.REAL) {
-      configurePathPlanner();
+    // if (Constants.kCurrentMode == Mode.REAL) {
+    // configurePathPlanner();
 
-      autoChooser = AutoBuilder.buildAutoChooser();
+    // // autoChooser = AutoBuilder.buildAutoChooser();
 
-      SmartDashboard.putData(autoChooser);
-    }
+    // // SmartDashboard.putData(autoChooser);
+    // }
   }
 
   private void configureBindings() {
     List.of(
-        new DefaultControls(driver, operator, drive, indexer, intake, shooter),
-        new DriverControls(driver, operator, drive, shooter, intake, indexer))
+            new DefaultControls(driver, operator, drive, indexer, intake, shooter),
+            new DriverControls(driver, operator, drive, shooter, intake, indexer))
         .forEach(Configurable::configure);
   }
 
@@ -144,10 +144,10 @@ public void robotPeriodic() {
             new OdometryObservation(
                 Timer.getTimestamp(),
                 new SwerveModulePosition[] {
-                    new SwerveModulePosition(),
-                    new SwerveModulePosition(),
-                    new SwerveModulePosition(),
-                    new SwerveModulePosition()
+                  new SwerveModulePosition(),
+                  new SwerveModulePosition(),
+                  new SwerveModulePosition(),
+                  new SwerveModulePosition()
                 },
                 drive.getRawGyroRotation()));
 
@@ -156,17 +156,40 @@ public void robotPeriodic() {
   }
 
   public Command getAutonomousCommand() {
-    if (Constants.kCurrentMode == Mode.REAL) {
-      return autoChooser.getSelected();
-    }
-    return Commands.print("No autonomous command configured");
+    return Commands.parallel(
+        shooter.trackTargetFlywheel(() -> RobotState.getInstance().getTurretTarget()),
+        shooter.trackTargetHood(() -> RobotState.getInstance().getTurretTarget()),
+        Commands.sequence(Commands.waitUntil(shooter::flywheelAtGoal), indexer.index()));
   }
 
   public void configurePathPlanner() {
-    NamedCommands.registerCommand("Shoot", shooter.shootAtTargetNoRotation(() -> RobotState.getInstance().getTurretTarget()));
+    // RobotConfig config;
+    // try {
+    // config = RobotConfig.fromGUISettings();
+
+    // AutoBuilder.configure(
+    // () -> RobotState.getInstance().getEstimatedPose(),
+    // (Pose2d pose) -> RobotState.getInstance().setPose(pose),
+    // () -> RobotState.getInstance().getRobotVelocity(),
+    // (speeds, feedforwards) -> drive.runVelocity(speeds),
+    // new PPHolonomicDriveController(new PIDConstants(5.0, 0, 0), new
+    // PIDConstants(5.0, 0,
+    // 0)),
+    // config,
+    // AllianceFlipUtil::shouldFlip,
+    // drive);
+
+    // } catch (Exception e) {
+    // e.printStackTrace();
+    // }
+
+    NamedCommands.registerCommand(
+        "Shoot", shooter.shootAtTargetNoRotation(() -> RobotState.getInstance().getTurretTarget()));
     NamedCommands.registerCommand("Index", indexer.index());
     NamedCommands.registerCommand("Intake", intake.intake());
-    NamedCommands.registerCommand("DeployIntake", intake.deployOpenLoop().withTimeout(Seconds.of(2)));
-    NamedCommands.registerCommand("RetractIntake", intake.retractOpenLoop().withTimeout(Seconds.of(2)));
-   }
+    NamedCommands.registerCommand(
+        "DeployIntake", intake.deployOpenLoop().withTimeout(Seconds.of(2)));
+    NamedCommands.registerCommand(
+        "RetractIntake", intake.retractOpenLoop().withTimeout(Seconds.of(2)));
+  }
 }
diff --git a/src/main/java/frc/robot/control/DefaultControls.java b/src/main/java/frc/robot/control/DefaultControls.java
index 704521a..85895a6 100644
--- a/src/main/java/frc/robot/control/DefaultControls.java
+++ b/src/main/java/frc/robot/control/DefaultControls.java
@@ -1,17 +1,14 @@
 package frc.robot.control;
 
-import java.util.function.Supplier;
-
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import frc.robot.RobotState;
 import frc.robot.commands.DriveCommands;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.indexer.Indexer;
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.shooter.Shooter;
-import frc.robot.subsystems.shooter.TrajectoryCalculator;
+import java.util.function.Supplier;
 
 public class DefaultControls implements Configurable {
 
@@ -38,10 +35,7 @@ public DefaultControls(
     this.shooter = shooter;
   }
 
-  /**
-   * Configure all default commands for the subsystems (e.g. includes joystick
-   * driving).
-   */
+  /** Configure all default commands for the subsystems (e.g. includes joystick driving). */
   @Override
   public void configure() {
     drive.setDefaultCommand(
@@ -50,8 +44,10 @@ public void configure() {
 
     Supplier targetPoseSupplier = () -> RobotState.getInstance().getTurretTarget();
     // Avoid the trench
-    shooter.setHoodDefaultCommand(shooter.trackTargetHood(targetPoseSupplier));
-    shooter.setTurretDefaultCommand(
-        shooter.trackTargetTurret(targetPoseSupplier));
+    shooter.setTurretDefaultCommand(shooter.trackTargetTurret(targetPoseSupplier));
+
+    shooter
+        .getHood()
+        .setDefaultCommand(new RunCommand(() -> shooter.getHood().setAngle(0), shooter.getHood()));
   }
 }
diff --git a/src/main/java/frc/robot/control/DriverControls.java b/src/main/java/frc/robot/control/DriverControls.java
index ef0e346..a1a90ca 100644
--- a/src/main/java/frc/robot/control/DriverControls.java
+++ b/src/main/java/frc/robot/control/DriverControls.java
@@ -36,7 +36,8 @@ public DriverControls(
 
   @Override
   public void configure() {
-    configureSingleController();
+    configureDriverControls();
+    configureOperatorControls();
   }
 
   private void configureDriverControls() {
@@ -77,7 +78,13 @@ private void configureDriverControls() {
   private void configureOperatorControls() {
     operator.leftBumper().and(operator.leftTrigger().negate()).whileTrue(intake.intake());
 
-    operator.rightBumper().whileTrue(shooter.setFlywheelVelocity(8500));
+    operator
+        .rightBumper()
+        .whileTrue(
+            shooter
+                .trackTargetFlywheel(() -> RobotState.getInstance().getTurretTarget())
+                .alongWith(
+                    shooter.trackTargetHood(() -> RobotState.getInstance().getTurretTarget())));
 
     operator.rightTrigger().whileTrue(indexer.index());
 
@@ -103,11 +110,9 @@ private void configureOperatorControls() {
                   shooter.setHoodOpenLoop(0);
                 }));
 
-    operator.leftTrigger().whileTrue(intake.outtake());
     operator.aCross().whileTrue(intake.outtake());
     operator.xSquare().whileTrue(intake.deployOpenLoop());
     operator.yTriangle().whileTrue(intake.retractOpenLoop());
-    operator.bCircle().whileTrue(shooter.setFlywheelVelocity(2000).alongWith(indexer.index()));
 
     // operator.aCross().whileTrue(shooter.shootAtTargetNoRotation(() ->
     // RobotState.getInstance().getTurretTarget()));
@@ -116,7 +121,13 @@ private void configureOperatorControls() {
 
   private void configureSingleController() {
 
-    driver.rightBumper().whileTrue(shooter.setFlywheelVelocity(3000));
+    driver
+        .rightBumper()
+        .whileTrue(
+            shooter
+                .trackTargetFlywheel(() -> RobotState.getInstance().getTurretTarget())
+                .alongWith(
+                    shooter.trackTargetHood(() -> RobotState.getInstance().getTurretTarget())));
     // // RB -> Shoot
     // driver
     // .rightBumper()
@@ -155,6 +166,5 @@ private void configureSingleController() {
 
     driver.leftTrigger().whileTrue(intake.outtake());
     driver.rightTrigger().whileTrue(intake.intake());
-    
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java
index d919ba5..a0c6291 100644
--- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java
+++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java
@@ -27,8 +27,8 @@ public void periodic() {
   public Command index() {
     return Commands.startEnd(
         () -> {
-          io.setThroatOpenLoop(-IndexerConstants.kThroatMotorSpeed);
-          io.setToungeOpenLoop(IndexerConstants.kThroatMotorSpeed);
+          io.setThroatOpenLoop(-IndexerConstants.kGutsMotorSpeed);
+          io.setToungeOpenLoop(IndexerConstants.kGutsMotorSpeed);
         },
         () -> {
           io.stop();
@@ -39,8 +39,8 @@ public Command index() {
   public Command indexReverse() {
     return Commands.startEnd(
         () -> {
-          io.setThroatOpenLoop(IndexerConstants.kThroatMotorSpeed);
-          io.setToungeOpenLoop(-IndexerConstants.kThroatMotorSpeed);
+          io.setThroatOpenLoop(IndexerConstants.kGutsMotorSpeed);
+          io.setToungeOpenLoop(-IndexerConstants.kGutsMotorSpeed);
         },
         () -> {
           io.stop();
diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java
index 211a8ca..ffbe7f5 100644
--- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java
+++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java
@@ -1,6 +1,6 @@
 package frc.robot.subsystems.indexer;
 
 public final class IndexerConstants {
-  public static final double kThroatMotorSpeed = 0.5;
+  public static final double kGutsMotorSpeed = 0.4;
   public static final double kToungeMotorSpeed = 0.5;
 }
diff --git a/src/main/java/frc/robot/subsystems/leds/LedConstants.java b/src/main/java/frc/robot/subsystems/leds/LedConstants.java
index 2457d2b..ed481c5 100644
--- a/src/main/java/frc/robot/subsystems/leds/LedConstants.java
+++ b/src/main/java/frc/robot/subsystems/leds/LedConstants.java
@@ -1,16 +1,16 @@
 package frc.robot.subsystems.leds;
 
 public final class LedConstants {
-  public static final int kPort = 1;
+  public static final int kPort = 0;
 
-  public static final int kFullLength = 5;
+  public static final int kFullLength = 30;
 
   public static final double kStartupBreathDuration = 1.0;
   public static final double kStrobeSlowDuration = 0.2;
   public static final double kBreatheFastDuration = 0.5;
   public static final double kBreatheSlowDuration = 1.0;
   public static final double kRainbowCycleLength = 25.0;
-  public static final double kRainbowDuration = 0.25;
+  public static final double kRainbowDuration = 0.5;
   public static final double kRainbowStrobeDuration = 0.2;
   public static final double kWaveExponent = 0.4;
   public static final double kWaveFastCycleLength = 25.0;
diff --git a/src/main/java/frc/robot/subsystems/leds/Leds.java b/src/main/java/frc/robot/subsystems/leds/Leds.java
index b09b633..e4f575c 100644
--- a/src/main/java/frc/robot/subsystems/leds/Leds.java
+++ b/src/main/java/frc/robot/subsystems/leds/Leds.java
@@ -44,11 +44,11 @@ private Leds() {
   @Override
   public void periodic() {
     if (RobotState.isAutonomous()) {
-      solid(LedSection.ALL, Color.kOrange);
+      rainbow(LedSection.ALL, LedConstants.kRainbowCycleLength, LedConstants.kRainbowDuration);
     } else if (RobotState.isDisabled()) {
-      breath(LedSection.ALL, Color.kRed, Color.kBlack, 3);
+      solidRGB(LedSection.ALL, 0, 255, 0);
     } else {
-      solid(LedSection.ALL, Color.kAqua);
+      solidRGB(LedSection.ALL, 100, 100, 100);
     }
     // solid(LedSection.TOP_LEFT_TURRET, Color.kLimeGreen);
     // solid(LedSection.BOTTOM_LEFT_TURRET, Color.kYellow);
@@ -63,6 +63,13 @@ public void solid(LedSection section, Color color) {
     }
   }
 
+  public void solidRGB(LedSection section, int r, int g, int b) {
+    Section s = section.getSection();
+    for (int i = s.start(); i < s.end(); i++) {
+      buffer.setRGB(i, r, g, b);
+    }
+  }
+
   public void strobe(LedSection section, Color c1, Color c2, double duration) {
     boolean useFirst = ((Timer.getTimestamp() % duration) / duration) > 0.5;
     solid(section, useFirst ? c1 : c2);
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index c3a90ed..6b6c785 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -4,7 +4,6 @@
 
 package frc.robot.subsystems.shooter;
 
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -64,13 +63,18 @@ public void applyCommandNoRotation(ShooterCommand cmd) {
   }
 
   public Command shootAtTargetRotation(Supplier targetSupplier) {
-    return Commands.run(
+    return Commands.runEnd(
         () -> {
           ShooterCommand cmd = TrajectoryCalculator.calculate(targetSupplier.get());
           flywheel.setVelocity(cmd.wheelRPM());
           hood.setAngle(cmd.hoodAngle());
           turret.setPosition(cmd.turretAngle());
         },
+        () -> {
+          flywheel.setOpenLoop(0);
+          hood.setOpenLoop(0);
+          turret.setOpenLoop(0);
+        },
         this,
         turret,
         hood,
@@ -78,17 +82,33 @@ public Command shootAtTargetRotation(Supplier targetSupplier) {
   }
 
   public Command shootAtTargetNoRotation(Supplier targetSupplier) {
-    return Commands.run(
+    return Commands.runEnd(
         () -> {
           ShooterCommand cmd = TrajectoryCalculator.calculate(targetSupplier.get());
           flywheel.setVelocity(cmd.wheelRPM());
           hood.setAngle(cmd.hoodAngle());
         },
+        () -> {
+          flywheel.setOpenLoop(0);
+          hood.setOpenLoop(0);
+        },
         this,
         hood,
         flywheel);
   }
 
+  public Command trackTargetFlywheel(Supplier targetSupplier) {
+    return Commands.runEnd(
+        () -> {
+          ShooterCommand cmd = TrajectoryCalculator.calculate(targetSupplier.get());
+          flywheel.setVelocity(cmd.wheelRPM());
+        },
+        () -> {
+          flywheel.setOpenLoop(0);
+        },
+        flywheel);
+  }
+
   public Command hoodDown() {
     return hood.down();
   }
@@ -136,4 +156,12 @@ public void setHoodDefaultCommand(Command defaultCommand) {
   public void setFlywheelDefaultCommand(Command defaultCommand) {
     flywheel.setDefaultCommand(defaultCommand);
   }
+
+  public Hood getHood() {
+    return hood;
+  }
+
+  public boolean flywheelAtGoal() {
+    return flywheel.atGoal();
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
index fac06ac..0d908fd 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -45,9 +45,9 @@ public static final class HoodConstants {
                         new Transform3d(
                             Inches.of(7.268715), Inches.of(0), Inches.of(0), new Rotation3d())));
 
-    public static final double kMinAngleRad = Units.degreesToRadians(0);
+    public static final double kMaxAngleRad = Units.degreesToRadians(0);
     // TODO: Tune
-    public static final double kMaxAngleRad = 3.9;
+    public static final double kMinAngleRad = -3.9;
   }
 
   public static final class FlywheelConstants {
diff --git a/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java
index 047032a..c8fe696 100644
--- a/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java
+++ b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java
@@ -24,10 +24,11 @@ public class TrajectoryCalculator {
 
   static {
     shooterTable.put(2.36, new TrajectoryParams(2000.0, 0, 0.45));
-    shooterTable.put(2.6, new TrajectoryParams(2250.0, -0.5, 0.52));
-    shooterTable.put(3.0, new TrajectoryParams(2500.0, -1.0, 0.60));
-    shooterTable.put(3.5, new TrajectoryParams(2600.0, -3.0, 0.68));
-    shooterTable.put(4.0, new TrajectoryParams(3000.0, -3.9, 0.76));
+    shooterTable.put(2.6, new TrajectoryParams(2100.0, -0.5, 0.52));
+    shooterTable.put(3.0, new TrajectoryParams(2150.0, -1.0, 0.60));
+    shooterTable.put(3.5, new TrajectoryParams(2350.0, -1.5, 0.68));
+    shooterTable.put(4.0, new TrajectoryParams(2500.0, -1.75, 0.76));
+    shooterTable.put(4.5, new TrajectoryParams(3000.0, -2, 0.76));
   }
 
   // ========== PUBLIC API ==========
diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
index 8347fb9..bc0e2f2 100644
--- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOSparkMax.java
@@ -43,7 +43,7 @@ public HoodIOSparkMax() {
 
     // TODO: Tune
     config.closedLoop.feedForward.kS(0.015 * 12);
-    config.closedLoop.p(0.5);
+    config.closedLoop.p(1);
 
     config.closedLoop.allowedClosedLoopError(HoodConstants.kAngleTolerance, ClosedLoopSlot.kSlot0);
 
@@ -53,7 +53,7 @@ public HoodIOSparkMax() {
         () ->
             motor.configure(
                 config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters));
-    tryUntilOk(motor, 5, () -> encoder.setPosition(0));
+    encoder.setPosition(0);
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
index bc58698..69b19e2 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
@@ -52,7 +52,7 @@ public TurretIOSparkMax() {
 
     // TODO: Tune
     config.closedLoop.feedForward.kS(0.025 * 12);
-    config.closedLoop.p(0.1);
+    config.closedLoop.p(0.15);
     config.closedLoop.d(0.01);
     config.closedLoop.allowedClosedLoopError(
         TurretConstants.kAngleTolerance, ClosedLoopSlot.kSlot0);

From 56e620206bbedd895162d8939eebec4c0bb5bf01 Mon Sep 17 00:00:00 2001
From: Maxwell Morgan 
Date: Fri, 20 Mar 2026 15:51:18 -0400
Subject: [PATCH 61/61] Update for comp #2

---
 src/deploy/pathplanner/navgrid.json           | 1575 -----------------
 src/main/java/frc/robot/RobotContainer.java   |    9 +-
 src/main/java/frc/robot/RobotState.java       |    4 +-
 .../frc/robot/control/DefaultControls.java    |    7 +-
 .../frc/robot/control/DriverControls.java     |   52 +-
 .../frc/robot/subsystems/drive/Drive.java     |    5 +-
 .../java/frc/robot/subsystems/leds/Leds.java  |    2 +-
 .../frc/robot/subsystems/shooter/Shooter.java |   15 +
 .../shooter/TrajectoryCalculator.java         |    4 +-
 .../subsystems/shooter/turret/Turret.java     |    4 +
 .../subsystems/shooter/turret/TurretIO.java   |    2 +
 .../shooter/turret/TurretIOSparkMax.java      |    5 +
 12 files changed, 87 insertions(+), 1597 deletions(-)
 delete mode 100644 src/deploy/pathplanner/navgrid.json

diff --git a/src/deploy/pathplanner/navgrid.json b/src/deploy/pathplanner/navgrid.json
deleted file mode 100644
index 660ca52..0000000
--- a/src/deploy/pathplanner/navgrid.json
+++ /dev/null
@@ -1,1575 +0,0 @@
-{
-  "field_size": {
-    "x": 16.54,
-    "y": 8.07
-  },
-  "nodeSizeMeters": 0.3,
-  "grid": [
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ]
-  ]
-}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 783788b..9c3c824 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -151,14 +151,14 @@ public void robotPeriodic() {
                 },
                 drive.getRawGyroRotation()));
 
-    targetField2d.setRobotPose(GeomUtil.toPose2d(RobotState.getInstance().getTurretTarget()));
+    targetField2d.setRobotPose(GeomUtil.toPose2d(RobotState.getInstance().getShooterTarget()));
     field2d.setRobotPose(RobotState.getInstance().getEstimatedPose());
   }
 
   public Command getAutonomousCommand() {
     return Commands.parallel(
-        shooter.trackTargetFlywheel(() -> RobotState.getInstance().getTurretTarget()),
-        shooter.trackTargetHood(() -> RobotState.getInstance().getTurretTarget()),
+        shooter.trackTargetFlywheel(() -> RobotState.getInstance().getShooterTarget()),
+        shooter.trackTargetHood(() -> RobotState.getInstance().getShooterTarget()),
         Commands.sequence(Commands.waitUntil(shooter::flywheelAtGoal), indexer.index()));
   }
 
@@ -184,7 +184,8 @@ public void configurePathPlanner() {
     // }
 
     NamedCommands.registerCommand(
-        "Shoot", shooter.shootAtTargetNoRotation(() -> RobotState.getInstance().getTurretTarget()));
+        "Shoot",
+        shooter.shootAtTargetNoRotation(() -> RobotState.getInstance().getShooterTarget()));
     NamedCommands.registerCommand("Index", indexer.index());
     NamedCommands.registerCommand("Intake", intake.intake());
     NamedCommands.registerCommand(
diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java
index e098ee0..262b987 100644
--- a/src/main/java/frc/robot/RobotState.java
+++ b/src/main/java/frc/robot/RobotState.java
@@ -135,8 +135,8 @@ public ChassisSpeeds getFieldVelocity() {
     return ChassisSpeeds.fromRobotRelativeSpeeds(robotVelocity, getRotation());
   }
 
-  public Translation2d getTurretTarget() {
-    Pose2d estimatedPose = getEstimatedPose();
+  public Translation2d getShooterTarget() {
+    // Pose2d estimatedPose = getEstimatedPose();
     // if (estimatedPose.getX()
     //     < AllianceFlipUtil.applyX(FieldConstants.LinesVertical.neutralZoneNear)) {
     //   if (estimatedPose.getY() > AllianceFlipUtil.applyY(FieldConstants.LinesHorizontal.center))
diff --git a/src/main/java/frc/robot/control/DefaultControls.java b/src/main/java/frc/robot/control/DefaultControls.java
index 85895a6..f246946 100644
--- a/src/main/java/frc/robot/control/DefaultControls.java
+++ b/src/main/java/frc/robot/control/DefaultControls.java
@@ -42,9 +42,12 @@ public void configure() {
         DriveCommands.joystickDrive(
             drive, () -> -driver.getLeftY(), () -> -driver.getLeftX(), () -> -driver.getRightX()));
 
-    Supplier targetPoseSupplier = () -> RobotState.getInstance().getTurretTarget();
+    Supplier targetPoseSupplier = () -> RobotState.getInstance().getShooterTarget();
     // Avoid the trench
-    shooter.setTurretDefaultCommand(shooter.trackTargetTurret(targetPoseSupplier));
+    shooter
+        .getTurret()
+        .setDefaultCommand(
+            new RunCommand(() -> shooter.getTurret().setOpenLoop(0), shooter.getTurret()));
 
     shooter
         .getHood()
diff --git a/src/main/java/frc/robot/control/DriverControls.java b/src/main/java/frc/robot/control/DriverControls.java
index a1a90ca..e5134a6 100644
--- a/src/main/java/frc/robot/control/DriverControls.java
+++ b/src/main/java/frc/robot/control/DriverControls.java
@@ -45,7 +45,7 @@ private void configureDriverControls() {
         .xSquare()
         .onTrue(
             Commands.runOnce(
-                () -> RobotState.getInstance().resetRotation(Rotation2d.kZero), drive));
+                () -> RobotState.getInstance().resetRotation(Rotation2d.kZero)).alongWith(drive.zeroYaw()));
     driver.bCircle().onTrue(Commands.runOnce(drive::stopWithX, drive));
 
     driver.dPadUp().whileTrue(DriveCommands.crabWalk(drive, Direction.NORTH));
@@ -81,10 +81,8 @@ private void configureOperatorControls() {
     operator
         .rightBumper()
         .whileTrue(
-            shooter
-                .trackTargetFlywheel(() -> RobotState.getInstance().getTurretTarget())
-                .alongWith(
-                    shooter.trackTargetHood(() -> RobotState.getInstance().getTurretTarget())));
+            shooter.trackAndShootAtTargetFullRealCommandLatestGoodUseThisOne(
+                () -> RobotState.getInstance().getShooterTarget()));
 
     operator.rightTrigger().whileTrue(indexer.index());
 
@@ -114,6 +112,25 @@ private void configureOperatorControls() {
     operator.xSquare().whileTrue(intake.deployOpenLoop());
     operator.yTriangle().whileTrue(intake.retractOpenLoop());
 
+    operator
+        .dPadLeft()
+        .whileTrue(
+            Commands.runEnd(
+                () -> shooter.getTurret().setOpenLoop(-0.05),
+                () -> shooter.getTurret().setOpenLoop(0),
+                shooter.getTurret()));
+    operator
+        .dPadRight()
+        .whileTrue(
+            Commands.runEnd(
+                () -> shooter.getTurret().setOpenLoop(0.05),
+                () -> shooter.getTurret().setOpenLoop(0),
+                shooter.getTurret()));
+
+    operator
+        .bCircle()
+        .onTrue(Commands.runOnce(() -> shooter.getTurret().zero(), shooter.getTurret()));
+
     // operator.aCross().whileTrue(shooter.shootAtTargetNoRotation(() ->
     // RobotState.getInstance().getTurretTarget()));
     // operator.aCross().and(shooter::readyToShoot).whileTrue(indexer.index());
@@ -124,10 +141,8 @@ private void configureSingleController() {
     driver
         .rightBumper()
         .whileTrue(
-            shooter
-                .trackTargetFlywheel(() -> RobotState.getInstance().getTurretTarget())
-                .alongWith(
-                    shooter.trackTargetHood(() -> RobotState.getInstance().getTurretTarget())));
+            shooter.trackAndShootAtTargetFullRealCommandLatestGoodUseThisOne(
+                () -> RobotState.getInstance().getShooterTarget()));
     // // RB -> Shoot
     // driver
     // .rightBumper()
@@ -166,5 +181,24 @@ private void configureSingleController() {
 
     driver.leftTrigger().whileTrue(intake.outtake());
     driver.rightTrigger().whileTrue(intake.intake());
+
+    driver
+        .dPadLeft()
+        .whileTrue(
+            Commands.runEnd(
+                () -> shooter.getTurret().setOpenLoop(-0.05),
+                () -> shooter.getTurret().setOpenLoop(0),
+                shooter.getTurret()));
+    driver
+        .dPadRight()
+        .whileTrue(
+            Commands.runEnd(
+                () -> shooter.getTurret().setOpenLoop(0.05),
+                () -> shooter.getTurret().setOpenLoop(0),
+                shooter.getTurret()));
+
+    driver
+        .yTriangle()
+        .onTrue(Commands.runOnce(() -> shooter.getTurret().zero(), shooter.getTurret()));
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
index 58dcebe..41f07c5 100644
--- a/src/main/java/frc/robot/subsystems/drive/Drive.java
+++ b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -22,6 +22,7 @@
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
@@ -218,8 +219,8 @@ public void setYaw(Rotation2d angle) {
   }
 
   /** Zeros the gyro yaw. */
-  public void zeroYaw() {
-    setYaw(Rotation2d.kZero);
+  public Command zeroYaw() {
+    return Commands.runOnce(() -> this.setYaw(Rotation2d.kZero), this);
   }
 
   /** Returns a command to run a quasistatic test in the specified direction. */
diff --git a/src/main/java/frc/robot/subsystems/leds/Leds.java b/src/main/java/frc/robot/subsystems/leds/Leds.java
index e4f575c..fa6f56e 100644
--- a/src/main/java/frc/robot/subsystems/leds/Leds.java
+++ b/src/main/java/frc/robot/subsystems/leds/Leds.java
@@ -48,7 +48,7 @@ public void periodic() {
     } else if (RobotState.isDisabled()) {
       solidRGB(LedSection.ALL, 0, 255, 0);
     } else {
-      solidRGB(LedSection.ALL, 100, 100, 100);
+      stripes(LedSection.ALL, List.of(Color.kRed, Color.kWhite, Color.kBlue), 5, 1);
     }
     // solid(LedSection.TOP_LEFT_TURRET, Color.kLimeGreen);
     // solid(LedSection.BOTTOM_LEFT_TURRET, Color.kYellow);
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 6b6c785..84f058e 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -62,6 +62,13 @@ public void applyCommandNoRotation(ShooterCommand cmd) {
     hood.setAngle(cmd.hoodAngle());
   }
 
+  public Command trackAndShootAtTargetFullRealCommandLatestGoodUseThisOne(
+      Supplier targetSupplier) {
+    // e
+    return trackTargetTurret(targetSupplier)
+        .alongWith(trackTargetHood(targetSupplier), trackTargetFlywheel(targetSupplier));
+  }
+
   public Command shootAtTargetRotation(Supplier targetSupplier) {
     return Commands.runEnd(
         () -> {
@@ -161,6 +168,14 @@ public Hood getHood() {
     return hood;
   }
 
+  public Flywheel getFlywheel() {
+    return flywheel;
+  }
+
+  public Turret getTurret() {
+    return turret;
+  }
+
   public boolean flywheelAtGoal() {
     return flywheel.atGoal();
   }
diff --git a/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java
index c8fe696..128b9e6 100644
--- a/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java
+++ b/src/main/java/frc/robot/subsystems/shooter/TrajectoryCalculator.java
@@ -43,11 +43,11 @@ public static ShooterCommand calculate(Translation2d targetLocation) {
   }
 
   public static double calculateRPM(Translation2d targetLocation, Pose2d robotPose) {
-    return shooterTable.get(targetLocation.getDistance(robotPose.getTranslation())).wheelRPM;
+    return 0.6 * shooterTable.get(targetLocation.getDistance(robotPose.getTranslation())).wheelRPM;
   }
 
   public static double calculateHoodAngle(Translation2d targetLocation, Pose2d robotPose) {
-    return shooterTable.get(targetLocation.getDistance(robotPose.getTranslation())).hoodAngle;
+    return 0.5 * shooterTable.get(targetLocation.getDistance(robotPose.getTranslation())).hoodAngle;
   }
 
   // ========== PRIVATE IMPLEMENTATION ==========
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
index 2182781..957b03e 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/Turret.java
@@ -134,4 +134,8 @@ public double getVelocity() {
   public boolean atGoal() {
     return atGoal;
   }
+
+  public void zero() {
+    io.zero();
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java
index 6fad939..0829552 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIO.java
@@ -29,4 +29,6 @@ public class TurretIOOutputs {
   default void updateInputs(TurretIOInputs inputs) {}
 
   default void applyOutputs(TurretIOOutputs outputs) {}
+
+  default void zero() {}
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
index 69b19e2..37d3c5a 100644
--- a/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
+++ b/src/main/java/frc/robot/subsystems/shooter/turret/TurretIOSparkMax.java
@@ -96,4 +96,9 @@ public void applyOutputs(TurretIOOutputs outputs) {
       }
     }
   }
+
+  @Override
+  public void zero() {
+    encoder.setPosition(0);
+  }
 }