[wpilib] Fix Mecanum and SwerveControllerCommand when desired rotatio…

…n passed (#3808)
wpilibsuite · Dec 20, 2021 · 9f8f330 · 9f8f330
1 parent 1ad3b1b
commit 9f8f330
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 6 deletions.
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
@@ -242,9 +242,11 @@ MecanumControllerCommand::MecanumControllerCommand(
 }
 
 void MecanumControllerCommand::Initialize() {
-  m_desiredRotation = [&] {
-    return m_trajectory.States().back().pose.Rotation();
-  };
+  if (m_desiredRotation == nullptr) {
+    m_desiredRotation = [&] {
+      return m_trajectory.States().back().pose.Rotation();
+    };
+  }
   m_prevTime = 0_s;
   auto initialState = m_trajectory.Sample(0_s);
 

diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc
@@ -81,9 +81,11 @@ SwerveControllerCommand<NumModules>::SwerveControllerCommand(
 
 template <size_t NumModules>
 void SwerveControllerCommand<NumModules>::Initialize() {
-  m_desiredRotation = [&] {
-    return m_trajectory.States().back().pose.Rotation();
-  };
+  if (m_desiredRotation == nullptr) {
+    m_desiredRotation = [&] {
+      return m_trajectory.States().back().pose.Rotation();
+    };
+  }
   m_timer.Reset();
   m_timer.Start();
 }