diff options
Diffstat (limited to 'src/main/java/frc/robot/subsystems')
| -rw-r--r-- | src/main/java/frc/robot/subsystems/Drivetrain.java | 13 |
1 files changed, 11 insertions, 2 deletions
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index c149477..557c7e3 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -175,11 +175,20 @@ public class Drivetrain extends SubsystemBase public Command sysIdQausistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutine.quasistatic(direction); + return Commands.race(m_sysIdRoutine.quasistatic(direction), + Commands.runOnce(() -> { + while(m_encoderL.getDistance() < 6 && + m_encoderR.getDistance() < 6); + })); + } public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutine.dynamic(direction); + return Commands.race(m_sysIdRoutine.dynamic(direction), + Commands.runOnce(() -> { + while(m_encoderL.getDistance() < 6 && + m_encoderR.getDistance() < 6); + })); } }
\ No newline at end of file |