diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 12a648d..b06bc74 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -97,8 +97,11 @@ public Drive( @Override public void runState() { - // Can't run in auto otherwise it will constantly tell drive not to drive in auto (and thats not good) - if (DriverStation.isTeleop()) { + // Can't run in auto otherwise it will constantly tell drive not to drive in auto (and thats not + // good) + + if (!DriverStation.isAutonomous()) AutoAlign.periodic(); + if (DriverStation.isTeleop() && getState() != DriveStates.AUTO_ALIGN) { drive( this, () -> Constants.controller.getLeftY(), @@ -107,6 +110,8 @@ public void runState() { getState().getRotationModifier(), getState().getTranslationModifier() ); + } else if (DriverStation.isTeleop() && getState() == DriveStates.AUTO_ALIGN) { + AutoAlign.calculateChassisSpeed(); } }