diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp index 96824cc1718..cfa3a6d3083 100644 --- 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 index ba63b9c31be..d1c6e787116 100644 --- 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::SwerveControllerCommand( template void SwerveControllerCommand::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(); }