Pose estimation, VisualPoseProvider setup, plus a little more cleanup of Drivetrain subsystem to have things make slightly more sense

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

@@ -5,6 +5,7 @@
 package frc.robot;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -18,7 +19,9 @@ public class RobotContainer {
   private CommandXboxController secondary;
 
   public RobotContainer() {
-    drivetrain = new Drivetrain();
+    // TODO Need to provide a real initial pose
+    // TODO Need to provide a real VisualPoseProvider, null means we're not using one at all
+    drivetrain = new Drivetrain(new Pose2d(), null);
 
     primary = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
     secondary = new CommandXboxController(OIConstants.kSecondaryXboxUSB);

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

@@ -2,6 +2,7 @@ package frc.robot.subsystems;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -20,68 +21,90 @@ import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.utilities.MAXSwerveModule;
 import frc.robot.utilities.SwerveUtils;
+import frc.robot.utilities.VisualPoseProvider;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.PIDCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Drivetrain extends SubsystemBase {
     // Create MAXSwerveModules
-    private final MAXSwerveModule m_frontLeft = new MAXSwerveModule(
+    private final MAXSwerveModule m_frontLeft;
-        DrivetrainConstants.kFrontLeftDrivingCanId,
+    private final MAXSwerveModule m_frontRight;
-        DrivetrainConstants.kFrontLeftTurningCanId,
+    private final MAXSwerveModule m_rearLeft;
-        DrivetrainConstants.kFrontLeftChassisAngularOffset
+    private final MAXSwerveModule m_rearRight; 
-    );
-
-    private final MAXSwerveModule m_frontRight = new MAXSwerveModule(
-        DrivetrainConstants.kFrontRightDrivingCanId,
-        DrivetrainConstants.kFrontRightTurningCanId,
-        DrivetrainConstants.kFrontRightChassisAngularOffset
-    );
-
-    private final MAXSwerveModule m_rearLeft = new MAXSwerveModule(
-        DrivetrainConstants.kRearLeftDrivingCanId,
-        DrivetrainConstants.kRearLeftTurningCanId,
-        DrivetrainConstants.kBackLeftChassisAngularOffset
-    );
-
-    private final MAXSwerveModule m_rearRight = new MAXSwerveModule(
-        DrivetrainConstants.kRearRightDrivingCanId,
-        DrivetrainConstants.kRearRightTurningCanId,
-        DrivetrainConstants.kBackRightChassisAngularOffset
-    );
 
     // The gyro sensor
-    private final AHRS m_gyro = new AHRS(SPI.Port.kMXP);
+    private final AHRS m_gyro;
 
-    // Slew rate filter variables for controlling lateral acceleration
+    private final SlewRateLimiter m_magLimiter;
-    private double m_currentRotation = 0.0;
+    private final SlewRateLimiter m_rotLimiter;
-    private double m_currentTranslationDir = 0.0;
-    private double m_currentTranslationMag = 0.0;
 
-    private SlewRateLimiter m_magLimiter = new SlewRateLimiter(DrivetrainConstants.kMagnitudeSlewRate);
+    private final VisualPoseProvider m_visualPoseProvider;
-    private SlewRateLimiter m_rotLimiter = new SlewRateLimiter(DrivetrainConstants.kRotationalSlewRate);
-    private double m_prevTime = WPIUtilJNI.now() * 1e-6;
 
     // Odometry class for tracking robot pose
-    SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(
+    SwerveDrivePoseEstimator m_poseEstimator;
-        DrivetrainConstants.kDriveKinematics,
+
-        Rotation2d.fromDegrees(getGyroAngle()),
+    // Slew rate filter variables for controlling lateral acceleration
-        new SwerveModulePosition[] {
+    private double m_currentRotation;
-            m_frontLeft.getPosition(),
+    private double m_currentTranslationDir;
-            m_frontRight.getPosition(),
+    private double m_currentTranslationMag;
-            m_rearLeft.getPosition(),
+
-            m_rearRight.getPosition()
+    private double m_prevTime;
-        }
-    );
 
     /** Creates a new DriveSubsystem. */
-    public Drivetrain() {
+    public Drivetrain(Pose2d initialPose, VisualPoseProvider visualPoseProvider) {
+        m_frontLeft = new MAXSwerveModule(
+            DrivetrainConstants.kFrontLeftDrivingCanId,
+            DrivetrainConstants.kFrontLeftTurningCanId,
+            DrivetrainConstants.kFrontLeftChassisAngularOffset
+        );
+        m_frontRight = new MAXSwerveModule(
+            DrivetrainConstants.kFrontRightDrivingCanId,
+            DrivetrainConstants.kFrontRightTurningCanId,
+            DrivetrainConstants.kFrontRightChassisAngularOffset
+        );
+
+        m_rearLeft = new MAXSwerveModule(
+            DrivetrainConstants.kRearLeftDrivingCanId,
+            DrivetrainConstants.kRearLeftTurningCanId,
+            DrivetrainConstants.kBackLeftChassisAngularOffset
+        );
+
+        m_rearRight = new MAXSwerveModule(
+            DrivetrainConstants.kRearRightDrivingCanId,
+            DrivetrainConstants.kRearRightTurningCanId,
+            DrivetrainConstants.kBackRightChassisAngularOffset
+        );
+
+        m_gyro = new AHRS(SPI.Port.kMXP);
+
+        m_magLimiter = new SlewRateLimiter(DrivetrainConstants.kMagnitudeSlewRate);
+        m_rotLimiter = new SlewRateLimiter(DrivetrainConstants.kRotationalSlewRate);
+
+        m_poseEstimator = new SwerveDrivePoseEstimator(
+            DrivetrainConstants.kDriveKinematics,
+            Rotation2d.fromDegrees(getGyroAngle()),
+            new SwerveModulePosition[] {
+                m_frontLeft.getPosition(),
+                m_frontRight.getPosition(),
+                m_rearLeft.getPosition(),
+                m_rearRight.getPosition()
+            },
+            initialPose
+        );
+
+        m_visualPoseProvider = visualPoseProvider;
+
+        m_currentRotation = 0.0;
+        m_currentTranslationDir = 0.0;
+        m_currentTranslationMag = 0.0;
+        m_prevTime =  WPIUtilJNI.now() * 1e-6;
     }
 
     @Override
     public void periodic() {
         // Update the odometry in the periodic block
-        m_odometry.update(
+        m_poseEstimator.update(
             Rotation2d.fromDegrees(getGyroAngle()),
             new SwerveModulePosition[] {
                 m_frontLeft.getPosition(),
@@ -90,6 +113,13 @@ public class Drivetrain extends SubsystemBase {
                 m_rearRight.getPosition()
             }
         );
+
+        if (m_visualPoseProvider != null) {
+            m_poseEstimator.addVisionMeasurement(
+                m_visualPoseProvider.getVisualPose().visualPose(), 
+                m_visualPoseProvider.getVisualPose().timestamp()
+            );
+        }
     }
 
     /**
@@ -98,15 +128,15 @@ public class Drivetrain extends SubsystemBase {
      * @return The pose.
      */
     public Pose2d getPose() {
-        return m_odometry.getPoseMeters();
+        return m_poseEstimator.getEstimatedPosition();
     }
 
     public double getPoseX(){
-        return m_odometry.getPoseMeters().getX();
+        return m_poseEstimator.getEstimatedPosition().getX();
     }
 
     public double getPoseY(){
-        return m_odometry.getPoseMeters().getY();
+        return m_poseEstimator.getEstimatedPosition().getY();
     }
 
     /**
@@ -115,7 +145,7 @@ public class Drivetrain extends SubsystemBase {
      * @param pose The pose to which to set the odometry.
      */
     public void resetOdometry(Pose2d pose) {
-        m_odometry.resetPosition(
+        m_poseEstimator.resetPosition(
             Rotation2d.fromDegrees(getGyroAngle()),
             new SwerveModulePosition[] {
                 m_frontLeft.getPosition(),

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

@@ -11,7 +11,7 @@ import com.revrobotics.SparkPIDController;
 import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.RelativeEncoder;
 
-import frc.robot.constants.ModuleConstants;
+import frc.robot.constants.SwerveModuleConstants;
 
 public class MAXSwerveModule {
 	private final CANSparkMax m_drivingSparkMax;
@@ -52,49 +52,49 @@ public class MAXSwerveModule {
 		// Apply position and velocity conversion factors for the driving encoder. The
 		// native units for position and velocity are rotations and RPM, respectively,
 		// but we want meters and meters per second to use with WPILib's swerve APIs.
-		m_drivingEncoder.setPositionConversionFactor(ModuleConstants.kDrivingEncoderPositionFactor);
+		m_drivingEncoder.setPositionConversionFactor(SwerveModuleConstants.kDrivingEncoderPositionFactor);
-		m_drivingEncoder.setVelocityConversionFactor(ModuleConstants.kDrivingEncoderVelocityFactor);
+		m_drivingEncoder.setVelocityConversionFactor(SwerveModuleConstants.kDrivingEncoderVelocityFactor);
 
 		// Apply position and velocity conversion factors for the turning encoder. We
 		// want these in radians and radians per second to use with WPILib's swerve
 		// APIs.
-		m_turningEncoder.setPositionConversionFactor(ModuleConstants.kTurningEncoderPositionFactor);
+		m_turningEncoder.setPositionConversionFactor(SwerveModuleConstants.kTurningEncoderPositionFactor);
-		m_turningEncoder.setVelocityConversionFactor(ModuleConstants.kTurningEncoderVelocityFactor);
+		m_turningEncoder.setVelocityConversionFactor(SwerveModuleConstants.kTurningEncoderVelocityFactor);
 
 		// Invert the turning encoder, since the output shaft rotates in the opposite direction of
 		// the steering motor in the MAXSwerve Module.
-		m_turningEncoder.setInverted(ModuleConstants.kTurningEncoderInverted);
+		m_turningEncoder.setInverted(SwerveModuleConstants.kTurningEncoderInverted);
 
 		// Enable PID wrap around for the turning motor. This will allow the PID
 		// controller to go through 0 to get to the setpoint i.e. going from 350 degrees
 		// to 10 degrees will go through 0 rather than the other direction which is a
 		// longer route.
 		m_turningPIDController.setPositionPIDWrappingEnabled(true);
-		m_turningPIDController.setPositionPIDWrappingMinInput(ModuleConstants.kTurningEncoderPositionPIDMinInput);
+		m_turningPIDController.setPositionPIDWrappingMinInput(SwerveModuleConstants.kTurningEncoderPositionPIDMinInput);
-		m_turningPIDController.setPositionPIDWrappingMaxInput(ModuleConstants.kTurningEncoderPositionPIDMaxInput);
+		m_turningPIDController.setPositionPIDWrappingMaxInput(SwerveModuleConstants.kTurningEncoderPositionPIDMaxInput);
 
 		// Set the PID gains for the driving motor. Note these are example gains, and you
 		// may need to tune them for your own robot!
-		m_drivingPIDController.setP(ModuleConstants.kDrivingP);
+		m_drivingPIDController.setP(SwerveModuleConstants.kDrivingP);
-		m_drivingPIDController.setI(ModuleConstants.kDrivingI);
+		m_drivingPIDController.setI(SwerveModuleConstants.kDrivingI);
-		m_drivingPIDController.setD(ModuleConstants.kDrivingD);
+		m_drivingPIDController.setD(SwerveModuleConstants.kDrivingD);
-		m_drivingPIDController.setFF(ModuleConstants.kDrivingFF);
+		m_drivingPIDController.setFF(SwerveModuleConstants.kDrivingFF);
-		m_drivingPIDController.setOutputRange(ModuleConstants.kDrivingMinOutput,
+		m_drivingPIDController.setOutputRange(SwerveModuleConstants.kDrivingMinOutput,
-			ModuleConstants.kDrivingMaxOutput);
+			SwerveModuleConstants.kDrivingMaxOutput);
 
 		// Set the PID gains for the turning motor. Note these are example gains, and you
 		// may need to tune them for your own robot!
-		m_turningPIDController.setP(ModuleConstants.kTurningP);
+		m_turningPIDController.setP(SwerveModuleConstants.kTurningP);
-		m_turningPIDController.setI(ModuleConstants.kTurningI);
+		m_turningPIDController.setI(SwerveModuleConstants.kTurningI);
-		m_turningPIDController.setD(ModuleConstants.kTurningD);
+		m_turningPIDController.setD(SwerveModuleConstants.kTurningD);
-		m_turningPIDController.setFF(ModuleConstants.kTurningFF);
+		m_turningPIDController.setFF(SwerveModuleConstants.kTurningFF);
-		m_turningPIDController.setOutputRange(ModuleConstants.kTurningMinOutput,
+		m_turningPIDController.setOutputRange(SwerveModuleConstants.kTurningMinOutput,
-			ModuleConstants.kTurningMaxOutput);
+			SwerveModuleConstants.kTurningMaxOutput);
 
-		m_drivingSparkMax.setIdleMode(ModuleConstants.kDrivingMotorIdleMode);
+		m_drivingSparkMax.setIdleMode(SwerveModuleConstants.kDrivingMotorIdleMode);
-		m_turningSparkMax.setIdleMode(ModuleConstants.kTurningMotorIdleMode);
+		m_turningSparkMax.setIdleMode(SwerveModuleConstants.kTurningMotorIdleMode);
-		m_drivingSparkMax.setSmartCurrentLimit(ModuleConstants.kDrivingMotorCurrentLimit);
+		m_drivingSparkMax.setSmartCurrentLimit(SwerveModuleConstants.kDrivingMotorCurrentLimit);
-		m_turningSparkMax.setSmartCurrentLimit(ModuleConstants.kTurningMotorCurrentLimit);
+		m_turningSparkMax.setSmartCurrentLimit(SwerveModuleConstants.kTurningMotorCurrentLimit);
 
 		// Save the SPARK MAX configurations. If a SPARK MAX browns out during
 		// operation, it will maintain the above configurations.

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

@@ -0,0 +1,9 @@
+package frc.robot.utilities;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+public interface VisualPoseProvider {
+    public record VisualPose(Pose2d visualPose, double timestamp) {}
+
+    public VisualPose getVisualPose();
+}