Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +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

11
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
View File

@@ -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