[command] Fix timing issue in RamseteCommand (#2871)

This issue only existed on the initial iteration. When timing is paused and stepped,
initialize() and execute() get called with the same timestamp the first time, which
would result in a divide by zero. All subsequent steps advance timing and only
call execute() so the time deltas are all set correctly.

Author: prateekma

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(); }