Add get current chassis speeds to swerve example

CrossTheRoadElec · Oct 26, 2023 · 8f7cdfc · 8f7cdfc
1 parent c470444
commit 8f7cdfc
Showing 1 changed file with 5 additions and 1 deletion.
diff --git 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);
+    }
 }