added littleton swerve code and cleaned up imports

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

@@ -0,0 +1,235 @@
+// Copyright (c) 2025 FRC 6328
+// 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 org.littletonrobotics.frc2025.subsystems.drive;
+
+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.Rotation2d;
+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.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.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import java.util.concurrent.locks.Lock;
+import java.util.concurrent.locks.ReentrantLock;
+import org.littletonrobotics.frc2025.Constants;
+import org.littletonrobotics.frc2025.Constants.Mode;
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+public class Drive extends SubsystemBase {
+  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 Alert gyroDisconnectedAlert =
+      new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError);
+
+  private SwerveDriveKinematics kinematics =
+      new SwerveDriveKinematics(DriveConstants.moduleTranslations);
+  private Rotation2d rawGyroRotation = new Rotation2d();
+  private SwerveModulePosition[] lastModulePositions = // For delta tracking
+      new SwerveModulePosition[] {
+        new SwerveModulePosition(),
+        new SwerveModulePosition(),
+        new SwerveModulePosition(),
+        new SwerveModulePosition()
+      };
+  private SwerveDrivePoseEstimator poseEstimator =
+      new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d());
+
+  public Drive(
+      GyroIO gyroIO,
+      ModuleIO flModuleIO,
+      ModuleIO frModuleIO,
+      ModuleIO blModuleIO,
+      ModuleIO brModuleIO) {
+    this.gyroIO = gyroIO;
+    modules[0] = new Module(flModuleIO, 0);
+    modules[1] = new Module(frModuleIO, 1);
+    modules[2] = new Module(blModuleIO, 2);
+    modules[3] = new Module(brModuleIO, 3);
+  }
+
+  @Override
+  public void periodic() {
+    odometryLock.lock(); // Prevents odometry updates while reading data
+    gyroIO.updateInputs(gyroInputs);
+    Logger.processInputs("Drive/Gyro", gyroInputs);
+    for (var module : modules) {
+      module.periodic();
+    }
+    odometryLock.unlock();
+
+    // Log empty setpoint states when disabled
+    if (DriverStation.isDisabled()) {
+      Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {});
+      Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {});
+    }
+
+    // Calculate odometry
+    // 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].getPosition();
+      moduleDeltas[moduleIndex] =
+          new SwerveModulePosition(
+              modulePositions[moduleIndex].distance - lastModulePositions[moduleIndex].distance,
+              modulePositions[moduleIndex].angle);
+      lastModulePositions[moduleIndex] = modulePositions[moduleIndex];
+    }
+    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));
+    }
+    poseEstimator.updateWithTime(Timer.getTimestamp(), rawGyroRotation, modulePositions);
+
+    // Update gyro alert
+    gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.getMode() != Mode.SIM);
+  }
+
+  /**
+   * Runs the drive at the desired velocity.
+   *
+   * @param speeds Speeds in meters/sec
+   */
+  public void runVelocity(ChassisSpeeds speeds) {
+    // Calculate module setpoints
+    ChassisSpeeds discreteSpeeds = speeds.discretize(Constants.loopPeriodSecs);
+    SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds);
+    SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DriveConstants.maxLinearSpeed);
+
+    // 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++) {
+      modules[i].runSetpoint(setpointStates[i]);
+    }
+
+    // Log optimized setpoints (runSetpoint mutates each state)
+    Logger.recordOutput("SwerveStates/SetpointsOptimized", setpointStates);
+  }
+
+  /** 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);
+    }
+  }
+
+  /** Stops the drive. */
+  public void stop() {
+    runVelocity(new ChassisSpeeds());
+  }
+
+  /**
+   * 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 void stopWithX() {
+    Rotation2d[] headings = new Rotation2d[4];
+    for (int i = 0; i < 4; i++) {
+      headings[i] = DriveConstants.moduleTranslations[i].getAngle();
+    }
+    kinematics.resetHeadings(headings);
+    stop();
+  }
+
+  /** Returns the module states (turn angles and drive velocities) for all of the modules. */
+  @AutoLogOutput(key = "SwerveStates/Measured")
+  private SwerveModuleState[] getModuleStates() {
+    SwerveModuleState[] states = new SwerveModuleState[4];
+    for (int i = 0; i < 4; i++) {
+      states[i] = modules[i].getState();
+    }
+    return states;
+  }
+
+  /** Returns the module positions (turn angles and drive positions) for all of the modules. */
+  private SwerveModulePosition[] getModulePositions() {
+    SwerveModulePosition[] states = new SwerveModulePosition[4];
+    for (int i = 0; i < 4; i++) {
+      states[i] = modules[i].getPosition();
+    }
+    return states;
+  }
+
+  /** 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 odometry pose. */
+  @AutoLogOutput(key = "Odometry/Robot")
+  public Pose2d getPose() {
+    return poseEstimator.getEstimatedPosition();
+  }
+
+  /** Returns the current odometry rotation. */
+  public Rotation2d getRotation() {
+    return getPose().getRotation();
+  }
+
+  /** Resets the current odometry pose. */
+  public void setPose(Pose2d pose) {
+    poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose);
+  }
+
+  /** Adds a new timestamped vision measurement. */
+  public void addVisionMeasurement(
+      Pose2d visionRobotPoseMeters,
+      double timestampSeconds,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    poseEstimator.addVisionMeasurement(
+        visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs);
+  }
+
+  /** Returns the maximum linear speed in meters per sec. */
+  public double getMaxLinearSpeedMetersPerSec() {
+    return DriveConstants.maxLinearSpeed;
+  }
+
+  /** Returns the maximum angular speed in radians per sec. */
+  public double getMaxAngularSpeedRadPerSec() {
+    return getMaxLinearSpeedMetersPerSec() / DriveConstants.driveBaseRadius;
+  }
+}

src/main/java/first/robot/subsystems/drive/DriveConstants.java

@@ -0,0 +1,96 @@
+// Copyright (c) 2025 FRC 6328
+// 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 org.littletonrobotics.frc2025.subsystems.drive;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import lombok.Builder;
+import org.littletonrobotics.frc2025.Constants;
+import org.littletonrobotics.frc2025.Constants.RobotType;
+
+public class DriveConstants {
+  public static final double trackWidthX = Units.inchesToMeters(20.75);
+  public static final double trackWidthY = Units.inchesToMeters(20.75);
+  public static final double driveBaseRadius = Math.hypot(trackWidthX / 2, trackWidthY / 2);
+  public static final double maxLinearSpeed = 4.69;
+  public static final double maxAngularSpeed = 4.69 / driveBaseRadius;
+  public static final double maxLinearAcceleration = 22.0;
+
+  public static final double driveKs = 5.0;
+  public static final double driveKv = 0.0;
+  public static final double driveKp = 35.0;
+  public static final double driveKd = 0.0;
+  public static final double turnKp = 4000.0;
+  public static final double turnKd = 50.0;
+
+  /** Includes bumpers! */
+  public static final double robotWidth =
+      Units.inchesToMeters(28.0) + 2 * Units.inchesToMeters(2.0);
+
+  public static final Translation2d[] moduleTranslations = {
+    new Translation2d(trackWidthX / 2, trackWidthY / 2),
+    new Translation2d(trackWidthX / 2, -trackWidthY / 2),
+    new Translation2d(-trackWidthX / 2, trackWidthY / 2),
+    new Translation2d(-trackWidthX / 2, -trackWidthY / 2)
+  };
+
+  public static final double wheelRadius = Units.inchesToMeters(1.9413001940413326);
+
+  public static final ModuleConfig[] moduleConfigs = {
+    // FL
+    ModuleConfig.builder()
+        .driveMotorId(12)
+        .turnMotorId(9)
+        .encoderChannel(2)
+        .encoderOffset(Rotation2d.fromRadians(0.9022009671847623))
+        .turnInverted(true)
+        .encoderInverted(false)
+        .build(),
+    // FR
+    ModuleConfig.builder()
+        .driveMotorId(2)
+        .turnMotorId(10)
+        .encoderChannel(3)
+        .encoderOffset(Rotation2d.fromRadians(1.6663099495963458))
+        .turnInverted(true)
+        .encoderInverted(false)
+        .build(),
+    // BL
+    ModuleConfig.builder()
+        .driveMotorId(15)
+        .turnMotorId(11)
+        .encoderChannel(4)
+        .encoderOffset(Rotation2d.fromRadians(-0.09896592242077659))
+        .turnInverted(true)
+        .encoderInverted(false)
+        .build(),
+    // BR
+    ModuleConfig.builder()
+        .driveMotorId(3)
+        .turnMotorId(8)
+        .encoderChannel(5)
+        .encoderOffset(Rotation2d.fromRadians(-3.051832863487227))
+        .turnInverted(true)
+        .encoderInverted(false)
+        .build()
+  };
+
+  public static class PigeonConstants {
+    public static final int id = Constants.getRobot() == RobotType.DEVBOT ? 3 : 30;
+  }
+
+  @Builder
+  public record ModuleConfig(
+      int driveMotorId,
+      int turnMotorId,
+      int encoderChannel,
+      Rotation2d encoderOffset,
+      boolean turnInverted,
+      boolean encoderInverted) {}
+}

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

@@ -0,0 +1,22 @@
+// Copyright (c) 2025 FRC 6328
+// 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 org.littletonrobotics.frc2025.subsystems.drive;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface GyroIO {
+  @AutoLog
+  public static class GyroIOInputs {
+    public boolean connected = false;
+    public Rotation2d yawPosition = new Rotation2d();
+    public double yawVelocityRadPerSec = 0.0;
+  }
+
+  public default void updateInputs(GyroIOInputs inputs) {}
+}

src/main/java/first/robot/subsystems/drive/GyroIOOnboardIMU.java

@@ -0,0 +1,22 @@
+// Copyright (c) 2025 FRC 6328
+// 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 org.littletonrobotics.frc2025.subsystems.drive;
+
+import edu.wpi.first.wpilibj.OnboardIMU;
+import edu.wpi.first.wpilibj.OnboardIMU.MountOrientation;
+
+public class GyroIOOnboardIMU implements GyroIO {
+  private final OnboardIMU imu = new OnboardIMU(MountOrientation.kFlat);
+
+  @Override
+  public void updateInputs(GyroIOInputs inputs) {
+    inputs.connected = true;
+    inputs.yawPosition = imu.getRotation2d();
+    inputs.yawVelocityRadPerSec = imu.getGyroRateZ();
+  }
+}