convert to robot relative speeds, works now!

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 2d5d4743f547ee197c98003bc6f7e74b7bd75bd9..d68fb5ea4a6ff57fb68fa82e4e583939744a78ff 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -13,6 +13,7 @@ import choreo.auto.AutoChooser;
 import choreo.auto.AutoFactory;
 import choreo.auto.AutoRoutine;
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
@@ -191,7 +192,7 @@ public class RobotContainer {
        autoFactory = new AutoFactory(
             drive::getPose,
             drive::resetOdometry,
-            sample -> drive.setChassisSpeeds(sample.getChassisSpeeds(), false),
+            sample -> drive.setChassisSpeeds(ChassisSpeeds.fromFieldRelativeSpeeds(sample.getChassisSpeeds(), drive.getYaw()), false),
             true,
             drive,
             (trajectory, startOrFinish) -> {