Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
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.
This commit is contained in:
Prateek Machiraju
2020-11-21 13:03:01 -05:00
committed by GitHub
parent 256e7904fd
commit f7f9087fb5
2 changed files with 20 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
View File

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