diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 5781bae..c7b9799 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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( diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a2b38e9..5dcbe54 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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 GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT), + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), + // 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 diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java index b9fd1be..5f7f285 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/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; diff --git a/src/main/java/frc4388/robot/commands/IfCommand.java b/src/main/java/frc4388/robot/commands/IfCommand.java deleted file mode 100644 index 0f5cd47..0000000 --- a/src/main/java/frc4388/robot/commands/IfCommand.java +++ /dev/null @@ -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(); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 7397386..388dc91 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/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; diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java index 5fad561..b903e17 100644 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ b/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") diff --git a/src/main/java/frc4388/utility/ReefPositionHelper.java b/src/main/java/frc4388/utility/ReefPositionHelper.java index e8cb0f2..a3ab63e 100644 --- a/src/main/java/frc4388/utility/ReefPositionHelper.java +++ b/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; }