refactor(odometry): Continue WIP.

src/main/java/frc/robot/odometry/GyroscopeIO.java → src/main/java/frc/lib/sensor/GyroscopeIO.java

@@ -1,9 +1,9 @@
-package frc.robot.odometry;
+package frc.lib.sensor;
 
-/** Gyroscope hardware interface. */
+/** Gyroscope interface. */
 public interface GyroscopeIO {
 
-  /** Gyroscope's hardware interface. */
+  /** Gyroscope interface. */
   public static class GyroscopeIOValues {
     /** Roll angle in rotations. */
     public double rollRotations = 0.0;

src/main/java/frc/robot/odometry/GyroscopeIOPigeon2.java → src/main/java/frc/lib/sensor/GyroscopeIOPigeon2.java

@@ -1,4 +1,4 @@
-package frc.robot.odometry;
+package frc.lib.sensor;
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
@@ -10,30 +10,27 @@
 /** Pigeon 2 gyroscope. */
 public class GyroscopeIOPigeon2 implements GyroscopeIO {
 
-  /** Hardware Pigeon 2. */
-  private final Pigeon2 pigeon2;
+  /** Pigeon 2. */
+  private final Pigeon2 gyroscope;
 
-  /** Pigeon 2's roll, pitch, and yaw status signals. */
+  /** Pigeon 2 status signals. */
   private final StatusSignal<Double> roll, pitch, yaw, rollVelocity, pitchVelocity, yawVelocity;
 
-  /** Creates a new Pigeon 2 gyroscope. */
   public GyroscopeIOPigeon2() {
-    pigeon2 = new Pigeon2(0, "swerve");
+    gyroscope = new Pigeon2(0, "swerve");
 
-    roll = pigeon2.getRoll();
-    pitch = pigeon2.getPitch();
-    yaw = pigeon2.getYaw();
+    roll = gyroscope.getRoll();
+    pitch = gyroscope.getPitch();
+    yaw = gyroscope.getYaw();
 
-    rollVelocity = pigeon2.getAngularVelocityXWorld();
-    pitchVelocity = pigeon2.getAngularVelocityYWorld();
-    yawVelocity = pigeon2.getAngularVelocityZWorld();
+    rollVelocity = gyroscope.getAngularVelocityXWorld();
+    pitchVelocity = gyroscope.getAngularVelocityYWorld();
+    yawVelocity = gyroscope.getAngularVelocityZWorld();
   }
 
   @Override
   public void configure() {
-    Pigeon2Configuration config = OdometryFactory.createGyroscopeConfig();
-
-    Configurator.configurePigeon2(pigeon2.getConfigurator(), config);
+    Configurator.configurePigeon2(gyroscope.getConfigurator(), new Pigeon2Configuration());
   }
 
   @Override
@@ -51,6 +48,6 @@ public void update(GyroscopeIOValues values) {
 
   @Override
   public void setYaw(double yawRotations) {
-    pigeon2.setYaw(Units.rotationsToDegrees(yawRotations));
+    gyroscope.setYaw(Units.rotationsToDegrees(yawRotations));
   }
 }

src/main/java/frc/lib/sensor/GyroscopeIOSim.java

@@ -0,0 +1,36 @@
+package frc.lib.sensor;
+
+import frc.robot.RobotConstants;
+import java.util.function.DoubleSupplier;
+
+/** Simulated gyroscope. */
+public class GyroscopeIOSim implements GyroscopeIO {
+
+  /** Yaw velocity supplier in rotations per second */
+  private final DoubleSupplier yawVelocityRotationsPerSecond;
+
+  /** Yaw in rotations. */
+  private double yawRotations = 0.0;
+
+  /** Creates a new simulated gyroscope. */
+  public GyroscopeIOSim(DoubleSupplier yawVelocityRotationsPerSecond) {
+    this.yawVelocityRotationsPerSecond = yawVelocityRotationsPerSecond;
+  }
+
+  @Override
+  public void configure() {}
+
+  @Override
+  public void update(GyroscopeIOValues values) {
+    yawRotations += yawVelocityRotationsPerSecond.getAsDouble() * RobotConstants.PERIODIC_DURATION;
+
+    values.yawRotations = yawRotations;
+
+    values.yawVelocityRotations = yawVelocityRotationsPerSecond.getAsDouble();
+  }
+
+  @Override
+  public void setYaw(double yawRotations) {
+    this.yawRotations = yawRotations;
+  }
+}

src/main/java/frc/robot/arm/ArmFactory.java

@@ -15,6 +15,7 @@ public class ArmFactory {
   /**
    * Creates the shoulder controller.
    *
+   * @param config the shoulder controller config.
    * @return the shoulder controller.
    */
   public static PositionControllerIO createShoulder(MechanismConfig config) {

src/main/java/frc/robot/intake/IntakeFactory.java

@@ -9,9 +9,15 @@
 import frc.robot.RobotConstants;
 import frc.robot.RobotConstants.Subsystem;
 
-/** Helper class for creating hardware for the intake subsystem. */
+/** Factory for creating intake subsystem hardware. */
 public class IntakeFactory {
 
+  /**
+   * Creates the front roller controller.
+   *
+   * @param config the front roller controller config.
+   * @return the front roller controller.
+   */
   public static VelocityControllerIO createFrontRoller(MechanismConfig config) {
     if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.INTAKE)) {
       return new VelocityControllerIOTalonFXPIDF(new CAN(50), config);
@@ -20,6 +26,12 @@ public static VelocityControllerIO createFrontRoller(MechanismConfig config) {
     return new VelocityControllerIOSim();
   }
 
+  /**
+   * Creates the back roller controller.
+   *
+   * @param config the back roller controller config.
+   * @return the back roller controller.
+   */
   public static VelocityControllerIO createBackRoller(MechanismConfig config) {
     if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.INTAKE)) {
       return new VelocityControllerIOTalonFXPIDF(new CAN(40), config);

src/main/java/frc/robot/odometry/Odometry.java

@@ -1,6 +1,5 @@
 package frc.robot.odometry;
 
-import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -16,17 +15,15 @@
 import frc.lib.AllianceFlipHelper;
 import frc.lib.Subsystem;
 import frc.lib.Telemetry;
-import frc.robot.odometry.GyroscopeIO.GyroscopeIOValues;
+import frc.lib.sensor.GyroscopeIO;
+import frc.lib.sensor.GyroscopeIO.GyroscopeIOValues;
 import frc.robot.swerve.Swerve;
-import java.util.ArrayList;
-import java.util.List;
-import java.util.function.Consumer;
 import java.util.function.Supplier;
 
-/** Subsystem class for the odometry subsystem. */
+/** Odometry subsystem. */
 public class Odometry extends Subsystem {
 
-  /** Instance variable for the odometry subsystem singleton. */
+  /** Odometry singleton. */
   private static Odometry instance = null;
 
   /** Gyroscope. */
@@ -47,10 +44,7 @@ public class Odometry extends Subsystem {
   /** Field. */
   private final Field2d field;
 
-  /** List of functions to be called when pose is manually updated. */
-  private final List<Consumer<Rotation2d>> yawUpdateConsumers;
-
-  /** Creates a new instance of the odometry subsystem. */
+  /** Initializes the odometry subsystem and configures odometry hardware. */
   private Odometry() {
     gyroscope = OdometryFactory.createGyroscope(this);
     gyroscope.configure();
@@ -61,21 +55,14 @@ private Odometry() {
 
     gyroscope.update(gyroscopeValues);
 
-    Rotation2d initialGyroRotation = Rotation2d.fromRotations(gyroscopeValues.yawRotations);
-
-    Pose2d initialPose = new Pose2d();
-
     swervePoseEstimator =
         new SwerveDrivePoseEstimator(
             Swerve.getInstance().getKinematics(),
-            initialGyroRotation,
+            Rotation2d.fromRotations(gyroscopeValues.yawRotations),
             swerveModulePositionsSupplier.get(),
-            initialPose);
-    swervePoseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.7, .7, 9999999));
+            new Pose2d());
 
     field = new Field2d();
-
-    yawUpdateConsumers = new ArrayList<Consumer<Rotation2d>>();
   }
 
   /**
@@ -142,6 +129,8 @@ public Pose2d getPosition() {
    * @return the rotation of the robot on the field where zero is away from the blue alliance wall.
    */
   public Rotation2d getFieldRelativeHeading() {
+    // TODO Doesn't obey WPILib's coordinate system
+
     return getPosition().getRotation();
   }
 
@@ -153,8 +142,6 @@ public Rotation2d getFieldRelativeHeading() {
    *     alliance.
    */
   public Rotation2d getDriverRelativeHeading() {
-    gyroscope.update(gyroscopeValues);
-
     return Rotation2d.fromRotations(gyroscopeValues.yawRotations);
   }
 
@@ -181,15 +168,6 @@ public void setRotation(Rotation2d rotation) {
     setPosition(new Pose2d(position.getTranslation(), rotation));
   }
 
-  /**
-   * Adds a consumer for when pose is manually updated.
-   *
-   * @param consumer consumer for when pose is manually updated.
-   */
-  public void onYawUpdate(Consumer<Rotation2d> consumer) {
-    yawUpdateConsumers.add(consumer);
-  }
-
   /**
    * Zeroes the driver-relative rotation of the robot.
    *
@@ -199,8 +177,6 @@ public Command tare() {
     return Commands.runOnce(
         () -> {
           gyroscope.setYaw(0.0);
-
-          yawUpdateConsumers.forEach(consumer -> consumer.accept(Rotation2d.fromDegrees(0)));
         });
   }
 

src/main/java/frc/robot/odometry/OdometryFactory.java

@@ -1,35 +1,25 @@
 package frc.robot.odometry;
 
-import com.ctre.phoenix6.configs.Pigeon2Configuration;
+import edu.wpi.first.math.util.Units;
+import frc.lib.sensor.GyroscopeIO;
+import frc.lib.sensor.GyroscopeIOPigeon2;
+import frc.lib.sensor.GyroscopeIOSim;
 import frc.robot.Robot;
 import frc.robot.RobotConstants;
 import frc.robot.RobotConstants.Subsystem;
 
-/** Helper class for creating hardware for the odometry subsystem. */
+/** Factory for creating odometry subsystem hardware. */
 public class OdometryFactory {
 
   /**
-   * Creates a gyroscope.
+   * Creates the gyroscope.
    *
-   * @return a gyroscope.
+   * @return the gyroscope.
    */
   public static GyroscopeIO createGyroscope(Odometry odometry) {
     if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.ODOMETRY))
       return new GyroscopeIOPigeon2();
 
-    return new GyroscopeIOSim(odometry);
-  }
-
-  /**
-   * Creates a gyroscope configuration.
-   *
-   * @return a gyroscope configuration.
-   */
-  public static Pigeon2Configuration createGyroscopeConfig() {
-    Pigeon2Configuration gyroscopeConfig = new Pigeon2Configuration();
-
-    gyroscopeConfig.Pigeon2Features.EnableCompass = false;
-
-    return gyroscopeConfig;
+    return new GyroscopeIOSim(() -> Units.radiansToRotations(odometry.getVelocity().dtheta));
   }
 }