WIP (squash later)

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

@@ -0,0 +1,44 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.subsystems.drive.DriveBase;
+
+import java.util.LinkedList;
+import java.util.List;
+import java.util.function.DoubleSupplier;
+
+public class DriveCommands {
+  /**
+   * Field relative drive command using two joysticks (controlling linear and angular velocities).
+   */
+  public static Command joystickDrive(DriveBase driveBase, DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier omegaSupplier) {
+    return Commands.run(() -> {
+
+    }, driveBase);
+  }
+
+  // /**
+  //  * 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() {}
+
+  /**
+   * Measures the velocity feedforward constants for the drive motors.
+   */
+  public static Command feedforwardCharacterization() {
+    List<Double> velocitySamples = new LinkedList<>();
+    List<Double> voltageSamples = new LinkedList<>();
+    Timer timer = new Timer();
+
+    return Commands.sequence(
+
+    );
+  }
+
+  // /** Measures the robot's wheel radius by spinning in a circle. */
+  // public static Command wheelRadiusCharacterization() {}
+}

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

@@ -0,0 +1,25 @@
+package frc.robot.subsystems.drive;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class DriveBase extends SubsystemBase {
+  // private final GyroIO gyroIO;
+  // private final Module[] modules = new Module[4]; // FL, FR, BL, BR
+  // private final Alert gyroDisconnectedAlert =
+  //     new Alert("Disconnected gyro, using kinematic approximation as fallback.", Alert.AlertType.kError);
+  //
+  //
+  // public static Translation2d[] getModuleTranslations(double trackWidthX, double trackWidthY) {
+  //   return new Translation2d[] {
+  //       new Translation2d(trackWidthX / 2.0, trackWidthY / 2.0),
+  //       new Translation2d(trackWidthX / 2.0, -trackWidthY / 2.0),
+  //       new Translation2d(-trackWidthX / 2.0, trackWidthY / 2.0),
+  //       new Translation2d(-trackWidthX / 2.0, -trackWidthY / 2.0)
+  //   };
+  // }
+
+  public void test() {
+  }
+}

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

@@ -0,0 +1,3 @@
+package frc.robot.subsystems.drive;
+
+public interface GyroIO {}

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

@@ -0,0 +1,3 @@
+package frc.robot.subsystems.drive;
+
+public class GyroIOPigeon2 {}

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

@@ -0,0 +1,3 @@
+package frc.robot.subsystems.drive;
+
+public class Module {}

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

@@ -0,0 +1,3 @@
+package frc.robot.subsystems.drive;
+
+public interface ModuleIO {}

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

@@ -0,0 +1,3 @@
+// package frc.robot.subsystems.drive;
+//
+// public class ModuleIOSim {}

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

@@ -0,0 +1,3 @@
+package frc.robot.subsystems.drive;
+
+public class ModuleIOSpark {}

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

@@ -0,0 +1,58 @@
+package frc.robot.subsystems.drive;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import org.littletonrobotics.junction.AutoLog;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Optional;
+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.Supplier;
+
+public class OdometryManager {
+  public static Lock odometryLock = new ReentrantLock();
+  public static double ODOMETRY_FREQUENCY_HZ = 200.0;
+
+  private static OdometryManager instance = null;
+
+  public static OdometryManager getInstance() {
+    if (instance == null) {
+      instance = new OdometryManager();
+    }
+    return instance;
+  }
+
+  private final Queue<Double> timestampQueue = new ArrayBlockingQueue<>(20);
+  private final List<ModuleSource> m_moduleSources = new ArrayList<>(4);
+  private GyroSource m_gyroSource = null;
+
+  public void registerModuleSource(Supplier<Optional<Double>> drivePositionSupplier, Supplier<Optional<Rotation2d>> turnAngleSupplier) {
+
+  }
+
+  public void registerGyroSource(Supplier<Optional<Rotation2d>> robotYawSupplier) {
+
+  }
+
+  private static void run() {
+
+  }
+
+  private record ModuleSource(
+      Queue<Double> drivePositionQueue,Supplier<Optional<Double>> drivePositionSupplier,
+      Queue<Rotation2d> turnAngleQueue,Supplier<Optional<Rotation2d>> turnAngleSupplier
+  ) {}
+  private record GyroSource(Queue<Rotation2d> robotYawQueue, Supplier<Optional<Rotation2d>> robotYawSupplier) {}
+
+  @AutoLog
+  public class TimestampInputs {
+    public double[] timestamps;
+
+    public Rotation2d[] gyroYaws;
+    public SwerveModulePosition[][] modulePositions;
+  }
+}

src/main/java/frc/robot/util/EqualsUtil.java

@@ -0,0 +1,22 @@
+package frc.robot.util;
+
+import edu.wpi.first.math.geometry.Twist2d;
+
+public class EqualsUtil {
+  public static boolean epsilonEquals(double a, double b, double epsilon) {
+    return (a - epsilon <= b) && (a + epsilon >= b);
+  }
+
+  public static boolean epsilonEquals(double a, double b) {
+    return epsilonEquals(a, b, 1e-9);
+  }
+
+  /** Extension methods for wpi geometry objects */
+  public static class GeomExtensions {
+    public static boolean epsilonEquals(Twist2d twist, Twist2d other) {
+      return EqualsUtil.epsilonEquals(twist.dx, other.dx)
+          && EqualsUtil.epsilonEquals(twist.dy, other.dy)
+          && EqualsUtil.epsilonEquals(twist.dtheta, other.dtheta);
+    }
+  }
+}

src/main/java/frc/robot/util/GeomUtil.java

@@ -0,0 +1,154 @@
+package frc.robot.util;
+
+import edu.wpi.first.math.geometry.*;
+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, new Rotation2d());
+  }
+
+  /**
+   * 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, new Rotation2d());
+  }
+
+  /**
+   * 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(new Translation2d(), 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, new Rotation2d());
+  }
+
+  /**
+   * 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(new Translation2d(), 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 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());
+  }
+
+  public static Pose3d toPose3d(Translation3d translation) {
+    return new Pose3d(translation, new Rotation3d());
+  }
+
+  /**
+   * 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);
+  }
+}