From 8f7cdfc277f644844418e4aba3d955a47bebce2d Mon Sep 17 00:00:00 2001
From: CoryNessCTR <cness@ctr-electronics.com>
Date: Thu, 26 Oct 2023 10:28:46 -0400
Subject: [PATCH] Add get current chassis speeds to swerve example

---
 .../src/main/java/frc/robot/CommandSwerveDrivetrain.java    | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/CommandSwerveDrivetrain.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/CommandSwerveDrivetrain.java
index d5cc6689..3684b547 100644
--- a/java/SwerveWithPathPlanner/src/main/java/frc/robot/CommandSwerveDrivetrain.java
+++ b/java/SwerveWithPathPlanner/src/main/java/frc/robot/CommandSwerveDrivetrain.java
@@ -38,7 +38,7 @@ private void configurePathPlanner() {
         AutoBuilder.configureHolonomic(
             ()->this.getState().Pose, // Supplier of current robot pose
             this::seedFieldRelative,  // Consumer for seeding pose against auto
-            ()->new ChassisSpeeds(),
+            this::getCurrentSpeeds,
             (speeds)->this.setControl(autoRequest.withSpeeds(speeds)), // Consumer of ChassisSpeeds to drive the robot
             new HolonomicPathFollowerConfig(new PIDConstants(10, 0, 0),
                                             new PIDConstants(10, 0, 0),
@@ -78,4 +78,8 @@ public void seedFieldRelative(Pose2d location) {
             m_stateLock.writeLock().unlock();
         }
     }
+
+    public ChassisSpeeds getCurrentSpeeds() {
+        return m_kinematics.toChassisSpeeds(getState().ModuleStates);
+    }
 }