controlle change

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