FINALLY FIXED SWERVE

Reviewed-on: #1

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2026.1.1"
+    id "edu.wpi.first.GradleRIO" version "2026.2.1"
 }
 
 java {

src/main/deploy/pathplanner/paths/Example Path.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 6.0
+      },
+      "prevControl": {
+        "x": 3.0,
+        "y": 6.0
+      },
+      "nextControl": 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": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/settings.json

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

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

@@ -4,37 +4,123 @@
 
 package frc.robot;
 
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.Timer;
+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.InstantCommand;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.constants.AutoConstants;
 import frc.robot.constants.OIConstants;
 import frc.robot.subsystems.Drivetrain;
+import frc.robot.utilities.Elastic;
+import frc.robot.utilities.Utilities;
 
 public class RobotContainer {
   private Drivetrain drivetrain;
 
   private CommandXboxController driver;
 
+  private SendableChooser<Command> autoChooser;
+
+  private Timer shiftTimer;
+
   public RobotContainer() {
     drivetrain = new Drivetrain();
 
     driver = new CommandXboxController(OIConstants.kDriverControllerPort);
 
+    shiftTimer = new Timer();
+    shiftTimer.reset();
+
     configureBindings();
   }
-
+  
   private void configureBindings() {
     drivetrain.setDefaultCommand(
       drivetrain.drive(
+        driver::getLeftY,
         driver::getLeftX, 
-        driver::getLeftY, 
         driver::getRightX, 
-        () -> true
+        () -> false
       )
     );
+
+    driver.start().and(driver.x()).whileTrue(drivetrain.runFrontLeft(1, 0));
+    driver.start().and(driver.y()).whileTrue(drivetrain.runFrontRight(1, 0));
+    driver.start().and(driver.a()).whileTrue(drivetrain.runRearLeft(1, 0));
+    driver.start().and(driver.b()).whileTrue(drivetrain.runRearRight(1, 0));
+    driver.start().negate().and(driver.x()).whileTrue(drivetrain.runFrontLeft(0, 45));
+    driver.start().negate().and(driver.y()).whileTrue(drivetrain.runFrontRight(0, 45));
+    driver.start().negate().and(driver.a()).whileTrue(drivetrain.runRearLeft(0, 45));
+    driver.start().negate().and(driver.b()).whileTrue(drivetrain.runRearRight(0, 45));
+    driver.rightBumper().whileTrue(drivetrain.setX());
+
+    //drivetrain.setDefaultCommand(drivetrain.disableOutputs());
+
+    configureShiftDisplay();
   }
 
   public Command getAutonomousCommand() {
-    return Commands.print("No autonomous command configured");
+    if(AutoConstants.kAutoConfigOk) {
+      return autoChooser.getSelected();
+    } else {
+      return new PrintCommand("Robot Config loading failed, autonomous disabled");
+    }
+  }
+
+  private void configureShiftDisplay() {
+    SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
+    
+    RobotModeTriggers.autonomous().onTrue(new InstantCommand(() -> {
+      shiftTimer.stop();
+      SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
+    }));
+
+    RobotModeTriggers.teleop().onTrue(new InstantCommand(() -> {
+      Elastic.selectTab(OIConstants.kTeleopTab);
+      shiftTimer.reset();
+      shiftTimer.start();
+    }));
+
+    new Trigger(() -> shiftTimer.get() <= 10).onTrue(new InstantCommand(() -> {
+      SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
+    }));
+
+    new Trigger(() -> shiftTimer.get() > 10 && shiftTimer.get() <= 35).onTrue(new InstantCommand(() -> {
+      SmartDashboard.putStringArray(
+        OIConstants.kCurrentActiveHub, 
+        Utilities.ShiftFirst() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
+      );
+    }));
+
+    new Trigger(() -> shiftTimer.get() > 35 && shiftTimer.get() <= 60).onTrue(new InstantCommand(() -> {
+      SmartDashboard.putStringArray(
+        OIConstants.kCurrentActiveHub, 
+        Utilities.ShiftFirst() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
+      );
+    }));
+
+    new Trigger(() -> shiftTimer.get() > 60 && shiftTimer.get() <= 85).onTrue(new InstantCommand(() -> {
+      SmartDashboard.putStringArray(
+        OIConstants.kCurrentActiveHub, 
+        Utilities.ShiftFirst() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
+      );
+    }));
+
+    new Trigger(() -> shiftTimer.get() > 85 && shiftTimer.get() <= 110).onTrue(new InstantCommand(() -> {
+      SmartDashboard.putStringArray(
+        OIConstants.kCurrentActiveHub, 
+        Utilities.ShiftFirst() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
+      );
+    }));
+
+    new Trigger(() -> shiftTimer.get() > 110).onTrue(new InstantCommand(() -> {
+      SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
+    }));
   }
 }

src/main/java/frc/robot/constants/AutoConstants.java

@@ -0,0 +1,62 @@
+package frc.robot.constants;
+
+import java.io.IOException;
+
+import org.json.simple.parser.ParseException;
+
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+import com.pathplanner.lib.path.PathConstraints;
+
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+
+// TODO This is all hold over from 2025, does any of it need to change?
+public class AutoConstants {
+    public static final double kMaxSpeedMetersPerSecond = 5;
+    public static final double kMaxAccelerationMetersPerSecondSquared = 4;
+    public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
+    public static final double kMaxAngularAccelerationRadiansPerSecondSquared = Math.PI;
+
+    public static final double kMaxSpeedMetersPerSecondAutoAlign = 2.5;
+
+    public static final double kPXYController = 3.5;
+    public static final double kPThetaController = 5;
+
+    public static final double kAlignPXYController = 2;
+    public static final double kAlignPThetaController = 5;
+
+    // Constraint for the motion profiled robot angle controller
+    public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
+        kMaxAngularSpeedRadiansPerSecond, kMaxAngularAccelerationRadiansPerSecondSquared);
+
+    public static final TrapezoidProfile.Constraints kAlignThetaControllerConstraints = new TrapezoidProfile.Constraints(
+        kMaxAngularSpeedRadiansPerSecond, kMaxAngularAccelerationRadiansPerSecondSquared);
+
+    // TODO This is a constant being managed like a static rewriteable variable
+    public static RobotConfig kRobotConfig;
+    public static boolean kAutoConfigOk;
+
+    public static final PPHolonomicDriveController kPPDriveController = new PPHolonomicDriveController(
+        new PIDConstants(kPXYController, 0, 0),
+        new PIDConstants(kPThetaController, 0, 0)
+    );
+
+    public static final PathConstraints kOnTheFlyConstraints = new PathConstraints(
+        kMaxSpeedMetersPerSecond, 
+        kMaxAccelerationMetersPerSecondSquared, 
+        kMaxAngularSpeedRadiansPerSecond, 
+        kMaxAngularAccelerationRadiansPerSecondSquared
+    );
+
+    static {
+        try {
+            kRobotConfig = RobotConfig.fromGUISettings();
+            kAutoConfigOk = true;
+        } catch (IOException | ParseException e) {
+            System.err.println("FAILED TO READ ROBOTCONFIG, WAS THE CONFIG SET UP IN PATHPLANNER?");
+            e.printStackTrace();
+            kAutoConfigOk = false;
+        }
+    }
+}

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -1,5 +1,6 @@
 package frc.robot.constants;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.util.Units;
@@ -9,41 +10,28 @@ public class DrivetrainConstants {
     public static final double kMaxSpeedMetersPerSecond = 4.125;
     public static final double kMaxAngularSpeed = 2 * Math.PI;
 
-    // TODO Replace zeros with real numbers
-    public static final double kTrackWidth = Units.inchesToMeters(0);
-    public static final double kWheelBase = Units.inchesToMeters(0);
+    public static final double kTrackWidth = Units.inchesToMeters(23.75);
+    public static final double kWheelBase = Units.inchesToMeters(18.75);
+ 
+    public static final double kFrontLeftMagEncoderOffset = 2.98;
+    public static final double kFrontRightMagEncoderOffset = 1.18;
+    public static final double kRearLeftMagEncoderOffset = 3.69;
+    public static final double kRearRightMagEncoderOffset = 2.54;
 
-    // TODO Replace zeros with real numbers
-    // These values should be determinable by writing the magnetic encoder output
-    // to the dashboard, and manually aligning all wheels such that the bevel gears
-    // on the side of the wheel all face left. Some known straight edge (like 1x1 or similar)
-    // should be used as the alignment medium. If done correctly, and the modules aren't disassembled,
-    // then these values should work throughout the season the first time they are set. 
-    public static final double kFrontLeftMagEncoderOffset = 0;
-    public static final double kFrontRightMagEncoderOffset = 0;
-    public static final double kRearLeftMagEncoderOffset = 0;
-    public static final double kRearRightMagEncoderOffset = 0;
-
-    // Kraken CAN IDs 
-    // TODO Real CAN IDs
     public static final int kFrontLeftDrivingCANID = 0;
-    public static final int kFrontRightDrivingCANID = 0;
-    public static final int kRearLeftDrivingCANID = 0;
-    public static final int kRearRightDrivingCANID = 0;
+    public static final int kFrontRightDrivingCANID = 3;
+    public static final int kRearLeftDrivingCANID = 1;
+    public static final int kRearRightDrivingCANID = 2;
 
-    // SparkMAX CAN IDs
-    // TODO Real CAN IDs
-    public static final int kFrontLeftTurningCANID = 0;
-    public static final int kFrontRightTurningCANID = 0;
-    public static final int kRearLeftTurningCANID = 0;
-    public static final int kRearRightTurningCANID = 0;
+    public static final int kFrontLeftTurningCANID = 8;
+    public static final int kFrontRightTurningCANID = 9;
+    public static final int kRearLeftTurningCANID = 7;
+    public static final int kRearRightTurningCANID = 6;
 
-    // Analog Encoder Input Ports 
-    // TODO Real Port IDs
-    public static final int kFrontLeftAnalogInPort = 0;
-    public static final int kFrontRightAnalogInPort = 0;
+    public static final int kFrontLeftAnalogInPort = 3;
+    public static final int kFrontRightAnalogInPort = 2;
     public static final int kRearLeftAnalogInPort = 0;
-    public static final int kRearRightAnalogInPort = 0;
+    public static final int kRearRightAnalogInPort = 1;
 
     public static final boolean kGyroReversed = true;
 

src/main/java/frc/robot/constants/ModuleConstants.java

@@ -15,8 +15,7 @@ import edu.wpi.first.math.util.Units;
 
 public class ModuleConstants {
     // DRIVING MOTOR CONFIG (Kraken)
-    // TODO Replace with something other than 0
-    public static final double kDrivingMotorReduction = 0;
+    public static final double kDrivingMotorReduction = (14.0 * 28.0 * 15.0) / (50 * 16 * 45);
 
     public static final double kDrivingMotorFeedSpeedRPS = KrakenMotorConstants.kFreeSpeedRPM / 60;
     public static final double kWheelDiameterMeters = Units.inchesToMeters(4);
@@ -43,13 +42,17 @@ public class ModuleConstants {
     public static final NeutralModeValue kDriveIdleMode = NeutralModeValue.Brake;
 
     // TURNING MOTOR CONFIG (NEO)
-
-    // TODO Hold over from 2025, adjust?
-    public static final double kTurningMotorReduction = 0;
+    public static final double kTurningMotorReduction = 150.0/7.0;
     public static final double kTurningFactor = 2 * Math.PI / kTurningMotorReduction;
+    // TODO Adjust? Let over from 2025
     public static final double kTurnP = 1;
     public static final double kTurnI = 0;
     public static final double kTurnD = 0;
+
+    public static final boolean kIsEncoderInverted = false;
+
+    // TODO How sensitive is too sensitive?
+    public static final double kAutoResetPositionDeadband = Units.degreesToRadians(.25);
     
     public static final int kTurnMotorCurrentLimit = 20;
 
@@ -87,9 +90,10 @@ public class ModuleConstants {
 
         turningConfig
             .idleMode(kTurnIdleMode)
-            .smartCurrentLimit(kTurnMotorCurrentLimit);
+            .smartCurrentLimit(kTurnMotorCurrentLimit)
+            .inverted(true);
         turningConfig.encoder
-            .inverted(true)
+            //.inverted(true)
             .positionConversionFactor(kTurningFactor)
             .velocityConversionFactor(kTurningFactor / 60.0);
         turningConfig.closedLoop
@@ -98,5 +102,6 @@ public class ModuleConstants {
             .outputRange(-1, 1)
             .positionWrappingEnabled(true)
             .positionWrappingInputRange(0, 2 * Math.PI);
+            
     }
 }

src/main/java/frc/robot/constants/OIConstants.java

@@ -1,5 +1,7 @@
 package frc.robot.constants;
 
+import edu.wpi.first.wpilibj.util.Color;
+
 public class OIConstants {
     public static final int kDriverControllerPort = 0;
     public static final int kOperatorControllerPort = 1;
@@ -7,4 +9,21 @@ public class OIConstants {
     public static final double kDriveDeadband = .01;
 
     public static final double kJoystickExponential = 3;
+
+    public static final String kTeleopTab = "Teleoperated";
+
+    public static final String kCurrentActiveHub = "Alliance Hub Currently Active";
+
+    public static final String[] kRedBlueDisplay = {
+        Color.kRed.toHexString(),
+        Color.kBlue.toHexString()
+    };
+
+    public static final String[] kRedDisplay = {
+        Color.kRed.toHexString()
+    };
+
+    public static final String[] kBlueDisplay = {
+        Color.kBlue.toHexString()
+    };
 }

src/main/java/frc/robot/constants/PhotonConstants.java

@@ -0,0 +1,17 @@
+package frc.robot.constants;
+
+import edu.wpi.first.math.geometry.Transform3d;
+
+public class PhotonConstants {
+    public static final String kCamera1Name = "pv1";
+    public static final String kCamera2Name = "pv2";
+
+    // TODO Need actual values for all of this
+    public static final Transform3d kCamera1RobotToCam = new Transform3d();
+    public static final Transform3d kCamera2RobotToCam = new Transform3d();
+
+    public static final double kCamera1HeightMeters = 0;
+    public static final double kCamera1PitchRadians = 0;
+    public static final double kCamera2HeightMeters = 0;
+    public static final double kCamera2PitchRadians = 0;
+}

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

@@ -1,14 +1,19 @@
 package frc.robot.subsystems;
 
+import java.util.Optional;
+import java.util.OptionalDouble;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
 import org.littletonrobotics.junction.Logger;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathPlannerPath;
 import com.studica.frc.AHRS;
 import com.studica.frc.AHRS.NavXComType;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -16,10 +21,14 @@ 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.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.constants.OIConstants;
+import frc.robot.utilities.PhotonVision;
 import frc.robot.utilities.SwerveModule;
 
 public class Drivetrain extends SubsystemBase {
@@ -32,38 +41,41 @@ public class Drivetrain extends SubsystemBase {
 
     private SwerveDrivePoseEstimator estimator;
 
+    private PhotonVision camera1;
+    private PhotonVision camera2;
+
     public Drivetrain() {
         frontLeft = new SwerveModule(
             "FrontLeft", 
             DrivetrainConstants.kFrontLeftDrivingCANID, 
-            DrivetrainConstants.kFrontLeftTurningCANID, 
+            DrivetrainConstants.kFrontLeftTurningCANID);/*, 
             DrivetrainConstants.kFrontLeftAnalogInPort, 
             DrivetrainConstants.kFrontLeftMagEncoderOffset
-        );
+        ); */
 
         frontRight = new SwerveModule(
             "FrontRight", 
             DrivetrainConstants.kFrontRightDrivingCANID, 
-            DrivetrainConstants.kFrontRightTurningCANID, 
+            DrivetrainConstants.kFrontRightTurningCANID);/*, 
             DrivetrainConstants.kFrontRightAnalogInPort, 
             DrivetrainConstants.kFrontRightMagEncoderOffset
-        );
+        );*/
 
         rearLeft = new SwerveModule(
             "RearLeft", 
             DrivetrainConstants.kRearLeftDrivingCANID, 
-            DrivetrainConstants.kRearLeftTurningCANID, 
+            DrivetrainConstants.kRearLeftTurningCANID);/*, 
             DrivetrainConstants.kRearLeftAnalogInPort, 
             DrivetrainConstants.kRearLeftMagEncoderOffset
-        );
+        ); */
 
         rearRight = new SwerveModule(
             "RearRight", 
             DrivetrainConstants.kRearRightDrivingCANID, 
-            DrivetrainConstants.kRearRightTurningCANID, 
+            DrivetrainConstants.kRearRightTurningCANID);/*, 
             DrivetrainConstants.kRearRightAnalogInPort, 
             DrivetrainConstants.kRearRightMagEncoderOffset
-        );
+        ); */
 
         gyro = new AHRS(NavXComType.kMXP_SPI);
 
@@ -79,6 +91,25 @@ public class Drivetrain extends SubsystemBase {
             },
             new Pose2d()
         );
+
+        if(AutoConstants.kAutoConfigOk) {
+            AutoBuilder.configure(
+                this::getPose, 
+                this::resetOdometry, 
+                this::getCurrentChassisSpeeds, 
+                (speeds, feedforwards) -> driveWithChassisSpeeds(speeds), 
+                AutoConstants.kPPDriveController, 
+                AutoConstants.kRobotConfig,
+                () -> {
+                    Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
+                        if (alliance.isPresent()) {
+                            return alliance.get() == DriverStation.Alliance.Red;
+                        }
+                        return false;
+                }, 
+                this
+            );
+        }
     }
 
     @Override
@@ -99,16 +130,91 @@ public class Drivetrain extends SubsystemBase {
         rearRight.periodic();
 
         Logger.recordOutput("Drivetrain/Pose", getPose());
+        Logger.recordOutput("Drivetrain/Gyro Angle", getGyroValue());
         Logger.recordOutput("Drivetrain/Heading", getHeading());
     }
 
+    public Command runFrontLeft(double staticSpeed, double staticAngleDegrees) {
+        return run(() -> {
+            frontLeft.setDesiredState(new SwerveModuleState(
+                staticSpeed, 
+                Rotation2d.fromDegrees(staticAngleDegrees)));
+        });
+    }
+    public Command runFrontRight(double staticSpeed, double staticAngleDegrees) {
+        return run(() -> {
+            frontRight.setDesiredState(new SwerveModuleState(
+                staticSpeed, 
+                Rotation2d.fromDegrees(staticAngleDegrees)));
+        });
+    }
+    public Command runRearLeft(double staticSpeed, double staticAngleDegrees) {
+        return run(() -> {
+            rearLeft.setDesiredState(new SwerveModuleState(
+                staticSpeed, 
+                Rotation2d.fromDegrees(staticAngleDegrees)));
+        });
+    }
+    public Command runRearRight(double staticSpeed, double staticAngleDegrees) {
+        return run(() -> {
+            rearRight.setDesiredState(new SwerveModuleState(
+                staticSpeed, 
+                Rotation2d.fromDegrees(staticAngleDegrees)));
+        });
+    }
+
+    public Command disableOutputs() {
+        return run(() -> {
+            frontLeft.disableOutput();
+            frontRight.disableOutput();
+            rearLeft.disableOutput();
+            rearRight.disableOutput();
+        });  
+    }
+
+    // TODO check both cameras
+    public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) {
+        if (camera1 == null) {
+            return new PrintCommand("Camera 1 not available");
+        }
+
+        // TODO The process variable is different here than what these constants are used for, may need to use something different
+        PIDController controller = new PIDController(
+            AutoConstants.kPThetaController, 
+            0, 
+            0
+        );
+
+        return runOnce(controller::reset).andThen(
+            drive(
+                xSpeed, 
+                ySpeed, 
+                () -> {
+                    OptionalDouble tagYaw = camera1.getTagYawByID(tagID);
+
+                    return (tagYaw.isEmpty() ? 0 : controller.calculate(tagYaw.getAsDouble(), 0));
+                },
+                () -> false
+           )
+        );
+    }
+
+    public Command drivePathPlannerPath(PathPlannerPath path) {
+        if(AutoConstants.kAutoConfigOk) {
+            return AutoBuilder.followPath(path);
+        } else {
+            return new PrintCommand("Robot Config loading failed, on the fly PathPlanner disabled");
+        }
+        
+    }
+
     public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, BooleanSupplier fieldRelative) {
-        // TODO Inversions? Specific Alliance code?
+        // TODO Specific Alliance code?
         return run(() -> {
             drive(
-                MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband), 
-                MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
-                MathUtil.applyDeadband(rotation.getAsDouble(), OIConstants.kDriveDeadband), 
+                -MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband), 
+                -MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
+                -MathUtil.applyDeadband(rotation.getAsDouble(), OIConstants.kDriveDeadband), 
                 fieldRelative.getAsBoolean()
             );
         });
@@ -130,6 +236,19 @@ public class Drivetrain extends SubsystemBase {
         rearRight.resetEncoders();
     }
 
+    public void resetOdometry(Pose2d pose) {
+        estimator.resetPosition(
+            Rotation2d.fromDegrees(getGyroValue()),
+            new SwerveModulePosition[] {
+                frontLeft.getPosition(),
+                frontRight.getPosition(),
+                rearLeft.getPosition(),
+                rearRight.getPosition()
+            },
+            pose
+        );
+    }
+
     public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRelative) {
         double p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2));
         double xSpeedDelivered = 0;

src/main/java/frc/robot/utilities/Elastic.java

@@ -0,0 +1,390 @@
+// Copyright (c) 2023-2026 Gold87 and other Elastic contributors
+// This software can be modified and/or shared under the terms
+// defined by the Elastic license:
+// https://github.com/Gold872/elastic_dashboard/blob/main/LICENSE
+
+package frc.robot.utilities;
+
+import com.fasterxml.jackson.annotation.JsonProperty;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.PubSubOption;
+import edu.wpi.first.networktables.StringPublisher;
+import edu.wpi.first.networktables.StringTopic;
+
+public final class Elastic {
+  private static final StringTopic notificationTopic =
+      NetworkTableInstance.getDefault().getStringTopic("/Elastic/RobotNotifications");
+  private static final StringPublisher notificationPublisher =
+      notificationTopic.publish(PubSubOption.sendAll(true), PubSubOption.keepDuplicates(true));
+  private static final StringTopic selectedTabTopic =
+      NetworkTableInstance.getDefault().getStringTopic("/Elastic/SelectedTab");
+  private static final StringPublisher selectedTabPublisher =
+      selectedTabTopic.publish(PubSubOption.keepDuplicates(true));
+  private static final ObjectMapper objectMapper = new ObjectMapper();
+
+  /**
+   * Represents the possible levels of notifications for the Elastic dashboard. These levels are
+   * used to indicate the severity or type of notification.
+   */
+  public enum NotificationLevel {
+    /** Informational Message */
+    INFO,
+    /** Warning message */
+    WARNING,
+    /** Error message */
+    ERROR
+  }
+
+  /**
+   * Sends an notification to the Elastic dashboard. The notification is serialized as a JSON string
+   * before being published.
+   *
+   * @param notification the {@link Notification} object containing notification details
+   */
+  public static void sendNotification(Notification notification) {
+    try {
+      notificationPublisher.set(objectMapper.writeValueAsString(notification));
+    } catch (JsonProcessingException e) {
+      e.printStackTrace();
+    }
+  }
+
+  /**
+   * Selects the tab of the dashboard with the given name. If no tab matches the name, this will
+   * have no effect on the widgets or tabs in view.
+   *
+   * <p>If the given name is a number, Elastic will select the tab whose index equals the number
+   * provided.
+   *
+   * @param tabName the name of the tab to select
+   */
+  public static void selectTab(String tabName) {
+    selectedTabPublisher.set(tabName);
+  }
+
+  /**
+   * Selects the tab of the dashboard at the given index. If this index is greater than or equal to
+   * the number of tabs, this will have no effect.
+   *
+   * @param tabIndex the index of the tab to select.
+   */
+  public static void selectTab(int tabIndex) {
+    selectTab(Integer.toString(tabIndex));
+  }
+
+  /**
+   * Represents an notification object to be sent to the Elastic dashboard. This object holds
+   * properties such as level, title, description, display time, and dimensions to control how the
+   * notification is displayed on the dashboard.
+   */
+  public static class Notification {
+    @JsonProperty("level")
+    private NotificationLevel level;
+
+    @JsonProperty("title")
+    private String title;
+
+    @JsonProperty("description")
+    private String description;
+
+    @JsonProperty("displayTime")
+    private int displayTimeMillis;
+
+    @JsonProperty("width")
+    private double width;
+
+    @JsonProperty("height")
+    private double height;
+
+    /**
+     * Creates a new Notification with all default parameters. This constructor is intended to be
+     * used with the chainable decorator methods
+     *
+     * <p>Title and description fields are empty.
+     */
+    public Notification() {
+      this(NotificationLevel.INFO, "", "");
+    }
+
+    /**
+     * Creates a new Notification with all properties specified.
+     *
+     * @param level the level of the notification (e.g., INFO, WARNING, ERROR)
+     * @param title the title text of the notification
+     * @param description the descriptive text of the notification
+     * @param displayTimeMillis the time in milliseconds for which the notification is displayed
+     * @param width the width of the notification display area
+     * @param height the height of the notification display area, inferred if below zero
+     */
+    public Notification(
+        NotificationLevel level,
+        String title,
+        String description,
+        int displayTimeMillis,
+        double width,
+        double height) {
+      this.level = level;
+      this.title = title;
+      this.displayTimeMillis = displayTimeMillis;
+      this.description = description;
+      this.height = height;
+      this.width = width;
+    }
+
+    /**
+     * Creates a new Notification with default display time and dimensions.
+     *
+     * @param level the level of the notification
+     * @param title the title text of the notification
+     * @param description the descriptive text of the notification
+     */
+    public Notification(NotificationLevel level, String title, String description) {
+      this(level, title, description, 3000, 350, -1);
+    }
+
+    /**
+     * Creates a new Notification with a specified display time and default dimensions.
+     *
+     * @param level the level of the notification
+     * @param title the title text of the notification
+     * @param description the descriptive text of the notification
+     * @param displayTimeMillis the display time in milliseconds
+     */
+    public Notification(
+        NotificationLevel level, String title, String description, int displayTimeMillis) {
+      this(level, title, description, displayTimeMillis, 350, -1);
+    }
+
+    /**
+     * Creates a new Notification with specified dimensions and default display time. If the height
+     * is below zero, it is automatically inferred based on screen size.
+     *
+     * @param level the level of the notification
+     * @param title the title text of the notification
+     * @param description the descriptive text of the notification
+     * @param width the width of the notification display area
+     * @param height the height of the notification display area, inferred if below zero
+     */
+    public Notification(
+        NotificationLevel level, String title, String description, double width, double height) {
+      this(level, title, description, 3000, width, height);
+    }
+
+    /**
+     * Updates the level of this notification
+     *
+     * @param level the level to set the notification to
+     */
+    public void setLevel(NotificationLevel level) {
+      this.level = level;
+    }
+
+    /**
+     * @return the level of this notification
+     */
+    public NotificationLevel getLevel() {
+      return level;
+    }
+
+    /**
+     * Updates the title of this notification
+     *
+     * @param title the title to set the notification to
+     */
+    public void setTitle(String title) {
+      this.title = title;
+    }
+
+    /**
+     * Gets the title of this notification
+     *
+     * @return the title of this notification
+     */
+    public String getTitle() {
+      return title;
+    }
+
+    /**
+     * Updates the description of this notification
+     *
+     * @param description the description to set the notification to
+     */
+    public void setDescription(String description) {
+      this.description = description;
+    }
+
+    public String getDescription() {
+      return description;
+    }
+
+    /**
+     * Updates the display time of the notification
+     *
+     * @param seconds the number of seconds to display the notification for
+     */
+    public void setDisplayTimeSeconds(double seconds) {
+      setDisplayTimeMillis((int) Math.round(seconds * 1000));
+    }
+
+    /**
+     * Updates the display time of the notification in milliseconds
+     *
+     * @param displayTimeMillis the number of milliseconds to display the notification for
+     */
+    public void setDisplayTimeMillis(int displayTimeMillis) {
+      this.displayTimeMillis = displayTimeMillis;
+    }
+
+    /**
+     * Gets the display time of the notification in milliseconds
+     *
+     * @return the number of milliseconds the notification is displayed for
+     */
+    public int getDisplayTimeMillis() {
+      return displayTimeMillis;
+    }
+
+    /**
+     * Updates the width of the notification
+     *
+     * @param width the width to set the notification to
+     */
+    public void setWidth(double width) {
+      this.width = width;
+    }
+
+    /**
+     * Gets the width of the notification
+     *
+     * @return the width of the notification
+     */
+    public double getWidth() {
+      return width;
+    }
+
+    /**
+     * Updates the height of the notification
+     *
+     * <p>If the height is set to -1, the height will be determined automatically by the dashboard
+     *
+     * @param height the height to set the notification to
+     */
+    public void setHeight(double height) {
+      this.height = height;
+    }
+
+    /**
+     * Gets the height of the notification
+     *
+     * @return the height of the notification
+     */
+    public double getHeight() {
+      return height;
+    }
+
+    /**
+     * Modifies the notification's level and returns itself to allow for method chaining
+     *
+     * @param level the level to set the notification to
+     * @return the current notification
+     */
+    public Notification withLevel(NotificationLevel level) {
+      this.level = level;
+      return this;
+    }
+
+    /**
+     * Modifies the notification's title and returns itself to allow for method chaining
+     *
+     * @param title the title to set the notification to
+     * @return the current notification
+     */
+    public Notification withTitle(String title) {
+      setTitle(title);
+      return this;
+    }
+
+    /**
+     * Modifies the notification's description and returns itself to allow for method chaining
+     *
+     * @param description the description to set the notification to
+     * @return the current notification
+     */
+    public Notification withDescription(String description) {
+      setDescription(description);
+      return this;
+    }
+
+    /**
+     * Modifies the notification's display time and returns itself to allow for method chaining
+     *
+     * @param seconds the number of seconds to display the notification for
+     * @return the current notification
+     */
+    public Notification withDisplaySeconds(double seconds) {
+      return withDisplayMilliseconds((int) Math.round(seconds * 1000));
+    }
+
+    /**
+     * Modifies the notification's display time and returns itself to allow for method chaining
+     *
+     * @param displayTimeMillis the number of milliseconds to display the notification for
+     * @return the current notification
+     */
+    public Notification withDisplayMilliseconds(int displayTimeMillis) {
+      setDisplayTimeMillis(displayTimeMillis);
+      return this;
+    }
+
+    /**
+     * Modifies the notification's width and returns itself to allow for method chaining
+     *
+     * @param width the width to set the notification to
+     * @return the current notification
+     */
+    public Notification withWidth(double width) {
+      setWidth(width);
+      return this;
+    }
+
+    /**
+     * Modifies the notification's height and returns itself to allow for method chaining
+     *
+     * @param height the height to set the notification to
+     * @return the current notification
+     */
+    public Notification withHeight(double height) {
+      setHeight(height);
+      return this;
+    }
+
+    /**
+     * Modifies the notification's height and returns itself to allow for method chaining
+     *
+     * <p>This will set the height to -1 to have it automatically determined by the dashboard
+     *
+     * @return the current notification
+     */
+    public Notification withAutomaticHeight() {
+      setHeight(-1);
+      return this;
+    }
+
+    /**
+     * Modifies the notification to disable the auto dismiss behavior
+     *
+     * <p>This sets the display time to 0 milliseconds
+     *
+     * <p>The auto dismiss behavior can be re-enabled by setting the display time to a number
+     * greater than 0
+     *
+     * @return the current notification
+     */
+    public Notification withNoAutoDismiss() {
+      setDisplayTimeMillis(0);
+      return this;
+    }
+  }
+}

src/main/java/frc/robot/utilities/PhotonVision.java

@@ -45,6 +45,8 @@ public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider {
         this.latestResult = null;
     }
 
+    // TODO This periodic method has to be called from somewhere, even though the cameras
+    // could be used in multiple other subsystems
     public void periodic() {
         // TODO Do we care about missed results? Probably not, if we're taking long enough to miss results something else is wrong
         List<PhotonPipelineResult> results = camera.getAllUnreadResults();

src/main/java/frc/robot/utilities/SwerveModule.java

@@ -1,4 +1,4 @@
-package frc.robot.utiltiies;
+package frc.robot.utilities;
 
 import org.littletonrobotics.junction.Logger;
 
@@ -12,6 +12,7 @@ import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
@@ -43,14 +44,64 @@ public class SwerveModule {
 
     private String moduleName;
 
+    private SwerveModuleState lastTargetState;
+    private SwerveModuleState lastTargetStateOptimized;
+
+    private boolean isAbsoluteEncoderDisabled;
+    private boolean turningEncoderAutoRezeroEnabled;
+
+    /**
+     * Builds the swerve module but with the Absolute Encoder disabled.
+     * 
+     * This constructor assumes you zeroed the swerve modules (faced all the bevel gears to the left)
+     * before booting up the robot.
+     * 
+     * @param moduleName The module name, Front Left, Front Right, etc.
+     * @param drivingCANID The CAN ID of the Kraken used to drive the module wheel
+     * @param turningCANID The CAN ID of the Spark MAX used to turn the module wheel
+     */
+    public SwerveModule(String moduleName, int drivingCANID, int turningCANID) {
+        this(moduleName, drivingCANID, turningCANID, -1, -1);
+    }
+
+    /**
+     * Builds the swerve module with the normal features, disables automatic rezeroing of the turning encoder
+     * from the absolute encoder. 
+     * 
+     * @param moduleName The module name, Front Left, Front Right, etc.
+     * @param drivingCANID The CAN ID of the Kraken used to drive the module wheel
+     * @param turningCANID The CAN ID of the Spark MAX used to turn the module wheel
+     * @param analogEncoderID The Analog In port ID for the Thrify Absolute Encoder
+     * @param analogEncoderOffset The angular offset for the absolute encoder to achieve 0 position on the module 
+     */
     public SwerveModule(String moduleName, int drivingCANID, int turningCANID, int analogEncoderID, double analogEncoderOffset) {
+        this(moduleName, drivingCANID, turningCANID, analogEncoderID, analogEncoderOffset, false);
+    }
+
+    /**
+     * Builds the swerve module with the normal features, and gives the option to enable automatic turning encoder rezeroing
+     * when the turning motor is not moving
+     * 
+     * @param moduleName The module name, Front Left, Front Right, etc.
+     * @param drivingCANID The CAN ID of the Kraken used to drive the module wheel
+     * @param turningCANID The CAN ID of the Spark MAX used to turn the module wheel
+     * @param analogEncoderID The Analog In port ID for the Thrify Absolute Encoder
+     * @param analogEncoderOffset The angular offset for the absolute encoder to achieve 0 position on the module 
+     * @param turningEncoderAutoRezeroEnabled Should the turning encoder in the NEO automatically rezero from the absolute encoder
+     */
+    public SwerveModule(String moduleName, int drivingCANID, int turningCANID, 
+        int analogEncoderID, double analogEncoderOffset, boolean turningEncoderAutoRezeroEnabled) {
+        isAbsoluteEncoderDisabled = (analogEncoderID == -1) || (analogEncoderOffset < 0);
+
         drive = new TalonFX(drivingCANID);
         turning = new SparkMax(turningCANID, MotorType.kBrushless);
 
         turningRelativeEncoder = turning.getEncoder();
 
-        turningAbsoluteEncoder = new AnalogEncoder(analogEncoderID, 2 * Math.PI, analogEncoderOffset);
-
+        if(!isAbsoluteEncoderDisabled) {
+            turningAbsoluteEncoder = new AnalogEncoder(analogEncoderID, 2 * Math.PI, analogEncoderOffset);
+        }
+        
         turningClosedLoopController = turning.getClosedLoopController();
 
         drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig);
@@ -65,37 +116,70 @@ public class SwerveModule {
             PersistMode.kPersistParameters
         );
 
-        turningRelativeEncoder.setPosition(turningAbsoluteEncoder.get());
+        driveVelocityRequest = new VelocityVoltage(0);
+
+        if(isAbsoluteEncoderDisabled){
+            turningRelativeEncoder.setPosition(0);
+        } else {
+            turningRelativeEncoder.setPosition(turningAbsoluteEncoder.get());
+        }
+        
         drive.setPosition(0);
 
+        this.lastTargetState = getState();
+        this.lastTargetStateOptimized = getState();
+
+        this.turningEncoderAutoRezeroEnabled = turningEncoderAutoRezeroEnabled;
+
         this.moduleName = "Drivetrain/Modules/" + moduleName;
     }
 
     public void periodic() {
-        Logger.recordOutput(moduleName + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get());
+        if(!isAbsoluteEncoderDisabled) {
+            Logger.recordOutput(moduleName + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get());
+        }
+        
+        Logger.recordOutput(moduleName + "/ModuleTargetState", lastTargetState);
+        Logger.recordOutput(moduleName + "/ModuleTargetStateOptimized", lastTargetStateOptimized);
         Logger.recordOutput(moduleName + "/SwerveModuleState", getState());
         Logger.recordOutput(moduleName + "/SwerveModulePosition", getPosition());
+        Logger.recordOutput(moduleName + "/RelativeEncoderPosition", getTurningEncoderPosition());
+
+        /*
+        if(!isAbsoluteEncoderDisabled && turningEncoderAutoRezeroEnabled) {
+            if(Math.abs(getState().angle.getRadians() - lastTargetState.angle.getRadians()) <= ModuleConstants.kAutoResetPositionDeadband) {
+                resetEncoders();
+            }
+        }*/
     }
 
     public SwerveModuleState getState() {
         return new SwerveModuleState(
             drive.getVelocity().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters, 
-            new Rotation2d(turningRelativeEncoder.getPosition())
+            new Rotation2d(getTurningEncoderPosition())
         );
     }
 
     public SwerveModulePosition getPosition() {
         return new SwerveModulePosition(
             drive.getPosition().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters, 
-            new Rotation2d(turningRelativeEncoder.getPosition())
+            new Rotation2d(getTurningEncoderPosition())
         );
     }
 
+    public void disableOutput() {
+        drive.disable();
+        turning.disable();
+    }
+
     public void setDesiredState(SwerveModuleState desiredState) {
+        lastTargetState = new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
         // TODO is this really necessary, the offset is managed by the Absolute Encoder
         // and its "source of truth" behavior in relation to the relative encoder
         // Probably doesn't *hurt* that it's here, but it may not be needed
-        desiredState.optimize(new Rotation2d(turningRelativeEncoder.getPosition()));
+        desiredState.optimize(new Rotation2d(getTurningEncoderPosition()));
+
+        lastTargetStateOptimized = desiredState;
 
         drive.setControl(
             driveVelocityRequest.withVelocity(
@@ -111,6 +195,10 @@ public class SwerveModule {
         );
     }
 
+    public double getTurningEncoderPosition() {
+        return turningRelativeEncoder.getPosition() * (ModuleConstants.kIsEncoderInverted ? -1 : 1);
+    }
+
     public void resetEncoders() {
         drive.setPosition(0);
 
@@ -118,6 +206,10 @@ public class SwerveModule {
     }
 
     public void zeroTurningEncoder() {
-        turningRelativeEncoder.setPosition(turningAbsoluteEncoder.get());
+        if(isAbsoluteEncoderDisabled) {
+            turningRelativeEncoder.setPosition(0);
+        } else {
+            turningRelativeEncoder.setPosition(turningAbsoluteEncoder.get());
+        }
     }
 }

src/main/java/frc/robot/utilities/Utilities.java

@@ -0,0 +1,80 @@
+package frc.robot.utilities;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+
+public class Utilities {
+    static String gameData;
+
+    static Boolean Blue = false;
+
+    static Boolean Red = false;
+
+    public static final double kG = -9.81;
+    
+    public static Alliance ShiftFirst() {
+        gameData = DriverStation.getGameSpecificMessage();
+
+
+        if(gameData.length() > 0) {
+            switch (gameData.charAt(0)) {
+            case 'B' :
+                return Alliance.Red;
+            case 'R' :
+                return Alliance.Blue;
+            default :
+            return null;
+            }
+        }
+        return null;
+    }
+
+    /**
+     * A ChatGPT possible hallucination related to calcuating whether a shot is possible
+     * for a given speed and change in X and Y position
+     * 
+     * Note that X in this scenario is the physical distance from the shooter exit to 
+     * target. Y is the change in height from the shooter exit to the target height
+     * 
+     * TODO Review ChatGPT's math more thoroughly, preferably with someone with fresher math skills
+     * 
+     * @param targetVMPS The target velocity of the shooter in Meters per Second
+     * @param deltaXM The "as the crow flies" distance between the shooter exit and the target
+     * @param deltaYM The height difference between the shooter exit and the target
+     * @return A true or false value indicating whether the shot is possible
+     */
+    public static boolean shotPossible(double targetVMPS, double deltaXM, double deltaYM) {
+        return Math.pow(targetVMPS, 4) >= 
+            -9.81 * (-9.81 * Math.pow(deltaXM, 2) + 2 * deltaYM * Math.pow(targetVMPS, 2));
+    }
+
+    /**
+     * A ChatGPT possible hallucination that calculates the angle required to make a shot for
+     * a given speed and change in X and Y position
+     * 
+     * Note that X in this scenario is the physical disatance from the shooter exit to
+     * target. Y is the change in height from the shooter exit to the target height
+     * 
+     * Setting softerShot to true changes the angle of attack to a soft, long range shot. False
+     * makes the shot more of a lob
+     * 
+     * TODO Review ChatGPT's math more thoroughly, preferably with someone with fresher math skills
+     * 
+     * @param targetVMPS The target velocity of the shooter in Meters per Second
+     * @param deltaXM The "as the crow flies" distance between the shooter exit and the target
+     * @param deltaYM The height difference between the shooter exit and the target
+     * @param softerShot True for a long range shot, false for a short range lob
+     * @return The angle required of the shooter to make the shot described by the input parameters
+     */
+    public static double shotAngle(double targetVMPS, double deltaXM, double deltaYM, boolean softerShot) {
+        double vPow2 = Math.pow(targetVMPS, 2);
+        double vPow4 = Math.pow(targetVMPS, 4);
+        double sqrtPiece = Math.sqrt(vPow4 - kG * (kG * Math.pow(deltaXM, 2) + 2 * deltaYM * vPow2));
+
+        if(softerShot) {
+            return Math.atan((vPow2 - sqrtPiece) / (kG * deltaXM));
+        } else {
+            return Math.atan((vPow2 + sqrtPiece) / (kG * deltaXM));
+        }
+    }
+}