diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java index c6a2b4c6bf..556dffa606 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java @@ -138,7 +138,7 @@ public class RamseteCommand extends CommandBase { @Override public void initialize() { - m_prevTime = 0; + m_prevTime = -1; var initialState = m_trajectory.sample(0); m_prevSpeeds = m_kinematics.toWheelSpeeds( new ChassisSpeeds(initialState.velocityMetersPerSecond, @@ -158,6 +158,12 @@ public class RamseteCommand extends CommandBase { double curTime = m_timer.get(); double dt = curTime - m_prevTime; + if (m_prevTime < 0) { + m_output.accept(0.0, 0.0); + m_prevTime = curTime; + return; + } + var targetWheelSpeeds = m_kinematics.toWheelSpeeds( m_follower.calculate(m_pose.get(), m_trajectory.sample(curTime))); @@ -189,9 +195,8 @@ public class RamseteCommand extends CommandBase { } m_output.accept(leftOutput, rightOutput); - - m_prevTime = curTime; m_prevSpeeds = targetWheelSpeeds; + m_prevTime = curTime; } @Override diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp index e2c56aba12..7b467a8b70 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp @@ -87,7 +87,7 @@ RamseteCommand::RamseteCommand( } void RamseteCommand::Initialize() { - m_prevTime = 0_s; + m_prevTime = -1_s; auto initialState = m_trajectory.Sample(0_s); m_prevSpeeds = m_kinematics.ToWheelSpeeds( frc::ChassisSpeeds{initialState.velocity, 0_mps, @@ -104,6 +104,16 @@ void RamseteCommand::Execute() { auto curTime = m_timer.Get(); auto dt = curTime - m_prevTime; + if (m_prevTime < 0_s) { + if (m_usePID) + m_outputVolts(0_V, 0_V); + else + m_outputVel(0_mps, 0_mps); + + m_prevTime = curTime; + return; + } + auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds( m_controller.Calculate(m_pose(), m_trajectory.Sample(curTime))); @@ -130,9 +140,8 @@ void RamseteCommand::Execute() { } else { m_outputVel(targetWheelSpeeds.left, targetWheelSpeeds.right); } - - m_prevTime = curTime; m_prevSpeeds = targetWheelSpeeds; + m_prevTime = curTime; } void RamseteCommand::End(bool interrupted) { m_timer.Stop(); }