Auto align Consistency improvements

src/main/java/frc4388/robot/Constants.java

@@ -229,29 +229,7 @@ public final class Constants {
             public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1);
             public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(5, 0, 0.1); // TODO: TEST
         }
-    
-        public static final class AutoConstants {
-            public static final Gains XY_GAINS = new Gains(3,0.01,0.0);
-            public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
 
-            public static final Trim X_OFFSET_TRIM =        new Trim("X Offset Trim",        Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0);
-            public static final Trim Y_OFFSET_TRIM =        new Trim("Y Offset Trim",        Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0);
-            public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0);
-            public static final Trim ARM_OFFSET_TRIM =      new Trim("ARM Offset Trim",      -ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 0);
-
-            public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
-            public static final int LIDAR_DIO_CHANNEL = 7;
-            public static final int LIDAR_MICROS_TO_CM = 10;
-            public static final int SECONDS_TO_MICROS = 1000000;
-                    
-            public static final double XY_TOLERANCE = 0.07; // Meters
-            public static final double ROT_TOLERANCE = 5; // Degrees
-                    
-            // public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d());
-            // public static final Pose2d targetpos = 
-        }
-    
-    
         public static final class Configurations {
             public static final double OPEN_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help.
             public static final double CLOSED_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help.
@@ -342,6 +320,48 @@ public final class Constants {
         public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
       }
     
+    public static final class LiDARConstants {
+        public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
+        public static final int LIDAR_DIO_CHANNEL = 7;
+        public static final int LIDAR_MICROS_TO_CM = 10;
+        public static final int SECONDS_TO_MICROS = 1000000;
+    }
+
+    public static final class AutoConstants {
+        public static final Gains XY_GAINS = new Gains(3,0.01,0.0);
+        public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
+
+        public static final Trim X_OFFSET_TRIM =        new Trim("X Offset Trim",        Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0);
+        public static final Trim Y_OFFSET_TRIM =        new Trim("Y Offset Trim",        Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0);
+        public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0);
+        public static final Trim ARM_OFFSET_TRIM =      new Trim("ARM Offset Trim",      -ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 0);
+                
+        public static final double XY_TOLERANCE = 0.07; // Meters
+        public static final double ROT_TOLERANCE = 5; // Degrees
+                
+        // X is tangent to reef side
+        // Y is normal to reef side
+        public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // This is from the field
+        public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16);
+
+        public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
+        public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
+        
+        public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
+        public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
+        
+        public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(1);
+
+
+        public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(3);
+
+        public static final int L4_DRIVE_TIME = 250; //Milliseconds
+        // public static final int L3_DRIVE_TIME = 500;
+        public static final int L2_DRIVE_TIME = 250; //Milliseconds
+
+        public static final int ALGAE_DRIVE_TIME = 500;
+    }
+
     public static final class VisionConstants { 
         public static final String LEFT_CAMERA_NAME = "CAMERA_LEFT";
         public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT";
@@ -360,31 +380,6 @@ public final class Constants {
 
     public static final class FieldConstants {
         public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
-        // public static final double HORISONTAL_SCORING_POSITION_OFFSET =  Units.inchesToMeters(9.5);
-
-        public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5);
-
-        // public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right
-
-        public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(16);
-        public static final double L4_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
-        public static final double L4_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
-        public static final double L3_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
-        public static final double L3_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
-
-        public static final double L2_ALGAE_REMOVAL = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(3);
-
-        
-        public static final double L2_SCORE_DISTANCE = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(1);
-
-        public static final int L4_DRIVE_TIME = 250; //Milliseconds
-        // public static final int L3_DRIVE_TIME = 500;
-        public static final int L2_DRIVE_TIME = 250; //Milliseconds
-
-        
-        public static final int ALGAE_DRIVE_TIME = 500;
-
-
 
         // Test april tag field layout
         // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(

src/main/java/frc4388/robot/RobotContainer.java

@@ -28,7 +28,7 @@ import frc4388.utility.controller.DeadbandedXboxController;
 import frc4388.robot.Constants.FieldConstants;
 import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
-import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
+import frc4388.robot.Constants.AutoConstants;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 
@@ -39,11 +39,10 @@ import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.Commands;
-
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 // Autos
 import frc4388.utility.controller.VirtualController;
 import frc4388.robot.commands.GotoLastApril;
-import frc4388.robot.commands.IfCommand;
 import frc4388.robot.commands.LidarAlign;
 import frc4388.robot.commands.MoveForTimeCommand;
 import frc4388.robot.commands.waitElevatorRefrence;
@@ -123,11 +122,11 @@ public class RobotContainer {
     // private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision);
     private Command AprilLidarAlignL4Right = new SequentialCommandGroup(
         new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
-
+        
-        // new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
+        new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
             new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
-            new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
+            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT)
-        // )),
+        ), () -> m_robotElevator.isL4Primed()),
 
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
         // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT),
@@ -135,7 +134,7 @@ public class RobotContainer {
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
 
-        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.RIGHT),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT),
         new LidarAlign(m_robotSwerveDrive, m_lidar),
 
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
@@ -143,7 +142,7 @@ public class RobotContainer {
         new waitEndefectorRefrence(m_robotElevator),
 
         new MoveForTimeCommand(m_robotSwerveDrive, 
-        new Translation2d(0,1), new Translation2d(), FieldConstants.L4_DRIVE_TIME, true),
+        new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
         
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
         new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
@@ -153,9 +152,11 @@ public class RobotContainer {
         new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
 
         // new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup(
+
+        new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
             new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
-            new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
+            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT)
-        // )),
+        ), () -> m_robotElevator.isL4Primed()),
         
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
         // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
@@ -163,7 +164,7 @@ public class RobotContainer {
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
 
-        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.LEFT),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT),
 
         new LidarAlign(m_robotSwerveDrive, m_lidar),
 
@@ -172,7 +173,7 @@ public class RobotContainer {
         new waitEndefectorRefrence(m_robotElevator),
 
         new MoveForTimeCommand(m_robotSwerveDrive, 
-            new Translation2d(0,1), new Translation2d(), FieldConstants.L4_DRIVE_TIME, true),
+            new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true),
 
 
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator),
@@ -182,15 +183,16 @@ public class RobotContainer {
     private Command AprilLidarAlignL3Left = new SequentialCommandGroup(
         new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
 
+        new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
         // new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
             new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
-            new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT),
+            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.LEFT)
-        // )),
+        ), () -> m_robotElevator.isL3Primed()),
 
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
 
-        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.LEFT),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.LEFT),
         new LidarAlign(m_robotSwerveDrive, m_lidar),
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
@@ -204,18 +206,19 @@ public class RobotContainer {
     private Command AprilLidarAlignL3Right = new SequentialCommandGroup(
         new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
         
+        new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
         // new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
             new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
-            new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT),
+            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.RIGHT)
-        // )),
+        ),() -> m_robotElevator.isL3Primed()),
         
-        new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
+        // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT),
+        // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT),
 
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
 
-        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.RIGHT),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT),
         
         new LidarAlign(m_robotSwerveDrive, m_lidar),
 
@@ -231,14 +234,14 @@ public class RobotContainer {
     
     private Command AprilLidarAlignL2Left = new SequentialCommandGroup(
         new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.LEFT),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT),
         
         new LidarAlign(m_robotSwerveDrive, m_lidar),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
-        new Translation2d(0,1), new Translation2d(), FieldConstants.L2_DRIVE_TIME, true),
+        new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
         new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
 
@@ -246,46 +249,28 @@ public class RobotContainer {
 
     private Command AprilLidarAlignL2Right = new SequentialCommandGroup(
         new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.RIGHT),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT),
-        
+
         new LidarAlign(m_robotSwerveDrive, m_lidar),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
-        new Translation2d(0,1), new Translation2d(), FieldConstants.L2_DRIVE_TIME, true),
+        new Translation2d(0,1), new Translation2d(), AutoConstants.L2_DRIVE_TIME, true),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
         new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
 
-    private Command placeCoral = new SequentialCommandGroup(
-        new InstantCommand(() -> m_robotSwerveDrive.stopModules()),
-        new InstantCommand(() -> System.out.println("Placing Some Coral")),
-        new WaitCommand(3),
-        new InstantCommand(() -> System.out.println("Done placing Some Coral"))
-    );
-
-    private Command aprilAlign = new SequentialCommandGroup(
-        new InstantCommand(() -> System.out.println("Aligning...")),
-        new WaitCommand(1)
-    );
-
-    private Command grabCoral = new SequentialCommandGroup(
-        new InstantCommand(() -> System.out.println("Yoinking some coral...")),
-        new WaitCommand(2),
-        new InstantCommand(() -> System.out.println("Done yoinking some coral..."))
-    );
-
     private Command leftAlgaeRemove = new SequentialCommandGroup(
         new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_ALGAE_REMOVAL - Units.inchesToMeters(1), Side.LEFT),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT),
         new LidarAlign(m_robotSwerveDrive, m_lidar),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
-        new Translation2d(0,1), new Translation2d(), 500, true),
+        new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
         new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
@@ -295,11 +280,11 @@ public class RobotContainer {
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_ALGAE_REMOVAL - Units.inchesToMeters(1), Side.LEFT),
+        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT),
         new LidarAlign(m_robotSwerveDrive, m_lidar),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
-        new Translation2d(0,1), new Translation2d(), 500, true),
+        new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator),
         new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
@@ -319,12 +304,10 @@ public class RobotContainer {
         NamedCommands.registerCommand("place-coral-right-l2", AprilLidarAlignL2Right);
 
         NamedCommands.registerCommand("lineup-no-arm", new SequentialCommandGroup(
-            new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT),
+            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT),
             new LidarAlign(m_robotSwerveDrive, m_lidar)
         ));
 
-        NamedCommands.registerCommand("grab-coral", grabCoral);
-
         configureButtonBindings();        
         configureVirtualButtonBindings();
         new DeferredBlock(() -> { // Called on first robot enable

src/main/java/frc4388/robot/commands/GotoLastApril.java

@@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
+import frc4388.robot.Constants.AutoConstants;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.subsystems.Vision;
 import frc4388.utility.Gains;

src/main/java/frc4388/robot/commands/IfCommand.java

@@ -1,33 +0,0 @@
-package frc4388.robot.commands;
-
-import java.time.Instant;
-import java.util.function.BooleanSupplier;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-
-// Command that is called if something is true
-public class IfCommand extends InstantCommand {
-    BooleanSupplier isTrue;
-    Command trueCommand;
-    Command falseCommand;
-
-    public IfCommand(BooleanSupplier isTrue, Command trueCommand, Command falseCommand){
-        this.isTrue = isTrue;
-        this.trueCommand = trueCommand;
-        this.falseCommand = falseCommand;
-    }
-
-    public IfCommand(BooleanSupplier isTrue, Command trueCommand){
-        this(isTrue, trueCommand, Commands.none());
-    }
-
-    @Override
-    public void execute() {
-        if(isTrue.getAsBoolean())
-            trueCommand.schedule();
-        else
-            falseCommand.schedule();
-    }
-}

src/main/java/frc4388/robot/subsystems/Elevator.java

@@ -20,7 +20,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.ElevatorConstants;
 import frc4388.robot.Constants.LEDConstants;
-import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
+import frc4388.robot.Constants.AutoConstants;
 import frc4388.robot.subsystems.LED;
 import frc4388.utility.LEDPatterns;
 import frc4388.utility.TimesNegativeOne;

src/main/java/frc4388/robot/subsystems/Lidar.java

@@ -7,7 +7,7 @@ import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
-import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
+import frc4388.robot.Constants.LiDARConstants;
 import frc4388.utility.Status;
 import frc4388.utility.Subsystem;
 import frc4388.utility.Status.ReportLevel;
@@ -16,7 +16,7 @@ import frc4388.utility.Status.ReportLevel;
 public class Lidar extends Subsystem {
 
     private double distance = -1;
-    Counter LidarPWM = new Counter(AutoConstants.LIDAR_DIO_CHANNEL);
+    Counter LidarPWM = new Counter(LiDARConstants.LIDAR_DIO_CHANNEL);
 
     public Lidar() {
         LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured
@@ -29,7 +29,7 @@ public class Lidar extends Subsystem {
         if(LidarPWM.get() < 1)
             distance = -1;
         else
-            distance = (LidarPWM.getPeriod() * AutoConstants.SECONDS_TO_MICROS) / AutoConstants.LIDAR_MICROS_TO_CM;
+            distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM;
     }
 
     public double getDistance(){
@@ -38,7 +38,7 @@ public class Lidar extends Subsystem {
 
     public boolean withinDistance(){
         if(distance == -1) return false;
-        return distance < AutoConstants.LIDAR_DETECT_DISTANCE;
+        return distance < LiDARConstants.LIDAR_DETECT_DISTANCE;
     }
 
     ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")

src/main/java/frc4388/utility/ReefPositionHelper.java

@@ -7,6 +7,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import frc4388.robot.Constants.AutoConstants;
 import frc4388.robot.Constants.FieldConstants;
 
 public class ReefPositionHelper {
@@ -79,9 +80,9 @@ public class ReefPositionHelper {
     public static double getSide(Side side){
         switch(side) {
             case LEFT:
-                return -(FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET);
+                return -(AutoConstants.X_SCORING_POSITION_OFFSET);
             case RIGHT:
-                return (FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET);
+                return (AutoConstants.X_SCORING_POSITION_OFFSET);
             case CENTER:
                 return 0;
         }