Some minor changes, either reducing duplicate code, or generalizing code

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

@@ -1,7 +1,11 @@
 package frc.robot.constants;
 
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
 
 public class DrivetrainConstants {
@@ -39,6 +43,11 @@ public class DrivetrainConstants {
     public static final double kXTranslationP = .5;
     public static final double kYTranslationP = .5;
 
+    // TODO How much do we trust gyro and encoders vs vision estimates.
+    // NOTE: Bigger values indicate LESS trust. Generally all three values for a given matrix should be the same
+    public static final Matrix<N3, N1> kSensorFusionOdometryStdDevs = VecBuilder.fill(0.1, 0.1, 0.1);
+    public static final Matrix<N3, N1> kVisionOdometryStdDevs = VecBuilder.fill(0.9, 0.9, 0.9);
+
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
     public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
         new Translation2d(kWheelBase / 2, kTrackWidth / 2),

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

@@ -14,6 +14,24 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import edu.wpi.first.math.util.Units;
 
 public class ModuleConstants {
+    public enum ModuleName {
+        kFrontLeft("FrontLeft"),
+        kFrontRight("FrontRight"),
+        kRearLeft("RearLeft"),
+        kRearRight("RearRight");
+
+        private String loggableName;
+
+        private ModuleName(String loggableName) {
+            this.loggableName = loggableName;
+        }
+
+        public String getLoggableName() {
+            return "Drivetrain/Modules/" + loggableName;
+        }
+        
+    }
+
     // DRIVING MOTOR CONFIG (Kraken)
     public static final double kDrivingMotorReduction = (14.0 * 28.0 * 15.0) / (50 * 16 * 45);
 

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

@@ -1,8 +1,10 @@
 package frc.robot.subsystems;
 
+import java.util.List;
 import java.util.Optional;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
 
 import org.littletonrobotics.junction.Logger;
 
@@ -27,6 +29,7 @@ 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.constants.ModuleConstants.ModuleName;
 import frc.robot.utilities.SwerveModule;
 import frc.robot.utilities.Utilities;
 import frc.robot.utilities.VisualPose;
@@ -45,7 +48,7 @@ public class Drivetrain extends SubsystemBase {
 
     public Drivetrain(Pose2d startupPose) {
         frontLeft = new SwerveModule(
-            "FrontLeft", 
+            ModuleName.kFrontLeft, 
             DrivetrainConstants.kFrontLeftDrivingCANID, 
             DrivetrainConstants.kFrontLeftTurningCANID, 
             DrivetrainConstants.kFrontLeftAnalogInPort, 
@@ -53,7 +56,7 @@ public class Drivetrain extends SubsystemBase {
         );
 
         frontRight = new SwerveModule(
-            "FrontRight", 
+            ModuleName.kFrontRight, 
             DrivetrainConstants.kFrontRightDrivingCANID, 
             DrivetrainConstants.kFrontRightTurningCANID, 
             DrivetrainConstants.kFrontRightAnalogInPort, 
@@ -61,7 +64,7 @@ public class Drivetrain extends SubsystemBase {
         );
 
         rearLeft = new SwerveModule(
-            "RearLeft", 
+            ModuleName.kRearLeft, 
             DrivetrainConstants.kRearLeftDrivingCANID, 
             DrivetrainConstants.kRearLeftTurningCANID, 
             DrivetrainConstants.kRearLeftAnalogInPort, 
@@ -69,7 +72,7 @@ public class Drivetrain extends SubsystemBase {
         );
 
         rearRight = new SwerveModule(
-            "RearRight", 
+            ModuleName.kRearRight, 
             DrivetrainConstants.kRearRightDrivingCANID, 
             DrivetrainConstants.kRearRightTurningCANID, 
             DrivetrainConstants.kRearRightAnalogInPort, 
@@ -96,7 +99,9 @@ public class Drivetrain extends SubsystemBase {
                 rearLeft.getPosition(),
                 rearRight.getPosition()
             },
-            startupPose != null ? startupPose : new Pose2d()
+            startupPose != null ? startupPose : new Pose2d(),
+            DrivetrainConstants.kSensorFusionOdometryStdDevs,
+            DrivetrainConstants.kVisionOdometryStdDevs
         );
 
         if(AutoConstants.kAutoConfigOk) {
@@ -141,32 +146,32 @@ public class Drivetrain extends SubsystemBase {
         Logger.recordOutput("Drivetrain/Heading", getHeadingDegrees());
     }
 
-    public Command runFrontLeft(double staticSpeed, double staticAngleDegrees) {
+    /**
+     * Can be used to run an individual module on the drive base a static speed while maintaining a static angle.
+     * 
+     * Good for diagnosing issues with swerve module configuration. Essentially useless otherwise.
+     * 
+     * @param name The ModuleName enumeration that indicates which module you want to control
+     * @param staticSpeed The static speed in Meters Per Second to spin the drive wheel at
+     * @param staticAngleDegrees The static angle in degrees that you want the wheel to face
+     * @return A complete Command structure that performs the specified action
+     */
+    public Command runIndividualModule(ModuleName name, double staticSpeed, double staticAngleDegrees) {
+        SwerveModule module = List.of(
+            frontLeft,
+            frontRight,
+            rearLeft,
+            rearRight
+        ).stream()
+        .filter((m) -> m.getModuleName() == name)
+        .findFirst()
+        .get();
+
         return run(() -> {
-            frontLeft.setDesiredState(new SwerveModuleState(
+            module.setDesiredState(new SwerveModuleState(
                 staticSpeed, 
-                Rotation2d.fromDegrees(staticAngleDegrees)));
+                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)));
         });
     }
 
@@ -235,12 +240,37 @@ public class Drivetrain extends SubsystemBase {
      * @return A complete Command structure that performs the specified action
      */
     public Command lockRotationToHub(DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) {
+        return lockRotationToSuppliedPose(
+            Utilities::getHubPose, 
+            xSpeed, 
+            ySpeed, 
+            rotate180
+        );
+    }
+
+    /**
+     * Locks the robots rotation to face a particular pose on the field
+     * 
+     * This method is <i>NOT</i> for autonomous, see rotateToPose
+     * 
+     * This method provides a field oriented mechanism of driving the robot, such that the robot
+     * is always facing the point on the field that is the Pose2d object being supplied. This 
+     * method assumes that the robots estimated pose is reasonably accurate.
+     * 
+     * @param poseSupplier A Supplier object, lambda, or method reference which consistently produces a Pose2d object to point towards
+     * @param xSpeed The X (forward/backward) translational speed of the robot
+     * @param ySpeed The Y (left/right) translational speed of the robot
+     * @param rotate180 When false, the front of the robot faces the supplied pose, when true, the back
+     *   of the robot faces the supplied pose
+     * @return A complete Command structure that performs the specified action
+     */
+    public Command lockRotationToSuppliedPose(Supplier<Pose2d> poseSupplier, DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) {
         return runOnce(yawRotationController::reset).andThen(
             drive(
                 xSpeed,
                 ySpeed, 
                 () -> {
-                    Pose2d faceTowards = Utilities.getHubPose();
+                    Pose2d faceTowards = poseSupplier.get();
 
                     Rotation2d targetRotation = new Rotation2d(
                         faceTowards.getX() - getPose().getX(), 

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

@@ -134,6 +134,7 @@ public class PhotonVision extends SubsystemBase {
         // This is based on what PhotonVision does for multitag Pose estimation
         // See PhotonPoseEstimator.multiTagOnCoprocStrategy
         // TODO This doesn't currently account for the offset of the tag relative to say the hub
+        // unclear if that offset amount will be important or not
         return Optional.of(Pose3d.kZero
             .plus(bestCameraToTarget.inverse())
             .relativeTo(CompetitionConstants.kTagLayout.getOrigin())

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

@@ -17,6 +17,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.AnalogEncoder;
 import frc.robot.constants.ModuleConstants;
+import frc.robot.constants.ModuleConstants.ModuleName;
 
 /*
  * This thread
@@ -30,6 +31,8 @@ import frc.robot.constants.ModuleConstants;
  * the controller closed loop controller. 
  */
 public class SwerveModule {
+    private ModuleName moduleName;
+
     private TalonFX drive;
     private SparkMax turning;
 
@@ -41,8 +44,6 @@ public class SwerveModule {
 
     private VelocityVoltage driveVelocityRequest;
 
-    private String moduleName;
-
     private SwerveModuleState lastTargetState;
     private SwerveModuleState lastTargetStateOptimized;
 
@@ -59,7 +60,7 @@ public class SwerveModule {
      * @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) {
+    public SwerveModule(ModuleName moduleName, int drivingCANID, int turningCANID) {
         this(moduleName, drivingCANID, turningCANID, -1, -1);
     }
 
@@ -73,7 +74,7 @@ public class SwerveModule {
      * @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) {
+    public SwerveModule(ModuleName moduleName, int drivingCANID, int turningCANID, int analogEncoderID, double analogEncoderOffset) {
         this(moduleName, drivingCANID, turningCANID, analogEncoderID, analogEncoderOffset, false);
     }
 
@@ -88,7 +89,7 @@ public class SwerveModule {
      * @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, 
+    public SwerveModule(ModuleName moduleName, int drivingCANID, int turningCANID, 
         int analogEncoderID, double analogEncoderOffset, boolean turningEncoderAutoRezeroEnabled) {
         isAbsoluteEncoderDisabled = (analogEncoderID == -1) || (analogEncoderOffset < 0);
 
@@ -130,20 +131,20 @@ public class SwerveModule {
 
         this.turningEncoderAutoRezeroEnabled = turningEncoderAutoRezeroEnabled;
 
-        this.moduleName = "Drivetrain/Modules/" + moduleName;
+        this.moduleName = moduleName;
     }
 
     public void periodic() {
         
         if(!isAbsoluteEncoderDisabled) {
-            Logger.recordOutput(moduleName + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get());
+            Logger.recordOutput(moduleName.getLoggableName() + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get());
         }
         
-        Logger.recordOutput(moduleName + "/ModuleTargetState", lastTargetState);
+        Logger.recordOutput(moduleName.getLoggableName() + "/ModuleTargetState", lastTargetState);
-        Logger.recordOutput(moduleName + "/ModuleTargetStateOptimized", lastTargetStateOptimized);
+        Logger.recordOutput(moduleName.getLoggableName() + "/ModuleTargetStateOptimized", lastTargetStateOptimized);
-        Logger.recordOutput(moduleName + "/SwerveModuleState", getState());
+        Logger.recordOutput(moduleName.getLoggableName() + "/SwerveModuleState", getState());
-        Logger.recordOutput(moduleName + "/SwerveModulePosition", getPosition());
+        Logger.recordOutput(moduleName.getLoggableName() + "/SwerveModulePosition", getPosition());
-        Logger.recordOutput(moduleName + "/RelativeEncoderPosition", getTurningEncoderPosition());
+        Logger.recordOutput(moduleName.getLoggableName() + "/RelativeEncoderPosition", getTurningEncoderPosition());
 
         // TODO Re-enable this? Was turned off when there was drivetrain issues
         // Now that there aren't, do we try this again?
@@ -155,6 +156,10 @@ public class SwerveModule {
         }*/
     }
 
+    public ModuleName getModuleName() {
+        return moduleName;
+    }
+
     public SwerveModuleState getState() {
         return new SwerveModuleState(
             drive.getVelocity().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters,