Constants

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

@@ -13,6 +13,8 @@
 
 package frc.robot;
 
+import static java.lang.Math.PI;
+
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
  * constants. This class should not be used for any other purpose. All constants should be declared
@@ -23,6 +25,12 @@
  */
 public final class Constants {
   public static final Mode currentMode = Mode.SIM;
+  // *START OF CONSTANTS FOR SWERVE* (STILL MISSING WHEEL POSITIONS - Im not on the cad thingy so i
+  // cant see it)
+  double WHEEL_RADIUS = 0.0508;
+  double WHEEL_CIRCUMFERENCE = WHEEL_RADIUS * 2 * PI;
+  double DRIVE_GEAR_RATIO = 6.75;
+  double STEERING_RATIO = (150 / 7);
 
   public static enum Mode {
     /** Running on a real robot. */

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

@@ -58,7 +58,7 @@ public RobotContainer() {
         // Real robot, instantiate hardware IO implementations
         drive =
             new Drive(
-                    new GyroIOPigeon2(),
+                new GyroIOPigeon2(),
                 new ModuleIOSparkMax(0),
                 new ModuleIOSparkMax(1),
                 new ModuleIOSparkMax(2),

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -36,8 +36,8 @@
 
 public class Drive extends SubsystemBase {
   private static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5);
-  private static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0);
-  private static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0);
+  private static final double TRACK_WIDTH_X = Units.inchesToMeters(20.75);
+  private static final double TRACK_WIDTH_Y = Units.inchesToMeters(20.75);
   private static final double DRIVE_BASE_RADIUS =
       Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0);
   private static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
@@ -77,10 +77,11 @@ MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()),
     Pathfinding.setPathfinder(new LocalADStarAK());
     //noinspection ToArrayCallWithZeroLengthArrayArgument
     PathPlannerLogging.setLogActivePathCallback(
-            (activePath) -> Logger.recordOutput(
-                    "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])));
+        (activePath) ->
+            Logger.recordOutput(
+                "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])));
     PathPlannerLogging.setLogTargetPoseCallback(
-            (targetPose) -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose));
+        (targetPose) -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose));
   }
 
   public void periodic() {
@@ -117,8 +118,8 @@ public void periodic() {
       // If the gyro is connected, replace the theta component of the twist
       // with the change in angle since the last loop cycle.
       twist =
-              new Twist2d(
-                      twist.dx, twist.dy, gyroInputs.yawPosition.minus(lastGyroRotation).getRadians());
+          new Twist2d(
+              twist.dx, twist.dy, gyroInputs.yawPosition.minus(lastGyroRotation).getRadians());
       lastGyroRotation = gyroInputs.yawPosition;
     }
     // Apply the twist (change since last loop cycle) to the current pose

src/main/java/frc/robot/subsystems/drive/GyroIO.java

@@ -17,8 +17,7 @@
 import org.littletonrobotics.junction.AutoLog;
 
 public interface GyroIO {
-  default void updateInputs(GyroIOInputs inputs) {
-  }
+  default void updateInputs(GyroIOInputs inputs) {}
 
   @AutoLog
   class GyroIOInputs {

src/main/java/frc/robot/subsystems/drive/Module.java

@@ -164,9 +164,7 @@ public SwerveModulePosition getPosition() {
     return new SwerveModulePosition(getPositionMeters(), getAngle());
   }
 
-  /**
-   * Returns the module position delta since the last call to this method.
-   */
+  /** Returns the module position delta since the last call to this method. */
   public SwerveModulePosition getPositionDelta() {
     var delta = new SwerveModulePosition(getPositionMeters() - lastPositionMeters, getAngle());
     lastPositionMeters = getPositionMeters();

src/main/java/frc/robot/subsystems/drive/ModuleIO.java

@@ -17,35 +17,20 @@
 import org.littletonrobotics.junction.AutoLog;
 
 public interface ModuleIO {
-  /**
-   * Updates the set of loggable inputs.
-   */
-  default void updateInputs(ModuleIOInputs inputs) {
-  }
+  /** Updates the set of loggable inputs. */
+  default void updateInputs(ModuleIOInputs inputs) {}
 
-  /**
-   * Run the drive motor at the specified voltage.
-   */
-  default void setDriveVoltage(double volts) {
-  }
+  /** Run the drive motor at the specified voltage. */
+  default void setDriveVoltage(double volts) {}
 
-  /**
-   * Run the turn motor at the specified voltage.
-   */
-  default void setTurnVoltage(double volts) {
-  }
+  /** Run the turn motor at the specified voltage. */
+  default void setTurnVoltage(double volts) {}
 
-  /**
-   * Enable or disable brake mode on the drive motor.
-   */
-  default void setDriveBrakeMode(boolean enable) {
-  }
+  /** Enable or disable brake mode on the drive motor. */
+  default void setDriveBrakeMode(boolean enable) {}
 
-  /**
-   * Enable or disable brake mode on the turn motor.
-   */
-  default void setTurnBrakeMode(boolean enable) {
-  }
+  /** Enable or disable brake mode on the turn motor. */
+  default void setTurnBrakeMode(boolean enable) {}
 
   @AutoLog
   class ModuleIOInputs {

src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java

@@ -48,6 +48,7 @@ public class ModuleIOSparkMax implements ModuleIO {
 
   @SuppressWarnings("FieldCanBeLocal")
   private final boolean isTurnMotorInverted = true;
+
   private final Rotation2d absoluteEncoderOffset;
 
   public ModuleIOSparkMax(int index) {

src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java

@@ -57,6 +57,7 @@ public class ModuleIOTalonFX implements ModuleIO {
   // Gear ratios for SDS MK4i L2, adjust as necessary
   @SuppressWarnings("FieldCanBeLocal")
   private final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0);
+
   @SuppressWarnings("FieldCanBeLocal")
   private final double TURN_GEAR_RATIO = 150.0 / 7.0;
 
@@ -119,7 +120,7 @@ public ModuleIOTalonFX(int index) {
     turnCurrent = turnTalon.getStatorCurrent();
 
     BaseStatusSignal.setUpdateFrequencyForAll(
-            100.0, drivePosition, turnPosition); // Required for odometry, use faster rate
+        100.0, drivePosition, turnPosition); // Required for odometry, use faster rate
     BaseStatusSignal.setUpdateFrequencyForAll(
         50.0,
         driveVelocity,