diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0100142..1b6adeb 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -107,9 +107,9 @@ public class RobotContainer { private Command autoCommand; private Command waitFeedStation = new waitSupplier(m_robotElevator::readyToMove); - private Command waitDebuger = new waitSupplier(m_driverXbox::getXButtonPressed); + // private Command waitDebuger = new waitSupplier(m_driverXbox::getYButtonPressed); // private Command waitDebugerManual = new waitSupplier(m_driverXbox::getYButtonPressed); - // private Command waitDebuger = new waitSupplier(() -> true); + private Command waitDebuger = new waitSupplier(() -> true); // private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); // private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, @@ -289,58 +289,42 @@ public class RobotContainer { private Command AprilLidarAlignL4LeftFullAuto = 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 DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true), + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true), // ), () -> m_robotElevator.isL4Primed()), - - waitDebuger.asProxy(), // 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, FieldConstants.L4_DISTANCE_2, Side.RIGHT), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - waitDebuger.asProxy(), - - new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true), waitDebuger.asProxy(), // new LidarAlign(m_robotSwerveDrive, reefLidar), // waitDebuger.asProxy(), - // new ParallelRaceGroup( - // new WaitCommand(1), - // new MoveUntilSuply( - // m_robotSwerveDrive, - // new Translation2d(0,-0.5), - // new Translation2d(), m_robotElevator::getEndeffectorLimit, true) - // ), - // new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive), - + new ParallelRaceGroup( + new WaitCommand(1), + new MoveUntilSuply( + m_robotSwerveDrive, + new Translation2d(0,-0.5), + new Translation2d(), m_robotElevator::getEndeffectorLimit, true) + ), + new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), - waitDebuger.asProxy(), - - new waitEndefectorRefrence(m_robotElevator), - waitDebuger.asProxy(), - - - - // new MoveForTimeCommand(m_robotSwerveDrive, - // new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), - - // // // new ConditionalCommand( - // // // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), - // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), - // () -> m_robotElevator.hasCoral()), - + new MoveForTimeCommand(m_robotSwerveDrive, + new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), + // new ConditionalCommand( + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + // () -> m_robotElevator.hasCoral()) new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); @@ -692,8 +676,8 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(AprilLidarAlignL4LeftFullAuto); + // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + // .onTrue(AprilLidarAlignL4LeftFullAuto); // ! /* Speed */ @@ -826,7 +810,7 @@ public class RobotContainer { // Auto Scoring new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode) - .onTrue(AprilLidarAlignL4LeftSemiAuto); + .onTrue(AprilLidarAlignL4LeftFullAuto); new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.8 && !operatorManualMode) .onTrue(AprilLidarAlignL4RightSemiAuto); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 85fb97a..f2695bd 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025RidgeScape"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 175; - public static final String GIT_SHA = "93ba351a198d2f83e0592eed3444465fc5b408f6"; - public static final String GIT_DATE = "2025-09-11 17:20:49 MDT"; + public static final int GIT_REVISION = 176; + public static final String GIT_SHA = "17239b8920efd45bc5a3b6a098baf4e5509fb558"; + public static final String GIT_DATE = "2025-09-13 11:20:40 MDT"; public static final String GIT_BRANCH = "advantagekit"; - public static final String BUILD_DATE = "2025-09-13 11:19:55 MDT"; - public static final long BUILD_UNIX_TIME = 1757783995171L; + public static final String BUILD_DATE = "2025-09-13 14:19:44 MDT"; + public static final long BUILD_UNIX_TIME = 1757794784689L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index a993e44..2b6f417 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -50,7 +50,7 @@ public final class Constants { public static final class AutoConstants { // public static final Gains XY_GAINS = new Gains(5,0.6,0.0); // public static final Gains XY_GAINS = new Gains(8,0,0.0); - public static final Gains XY_GAINS = new Gains(0.8,0,0.0); + public static final Gains XY_GAINS = new Gains(3,0,1.0); // public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP); // public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI); // public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD); @@ -79,8 +79,8 @@ public final class Constants { public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16+1); public static final double HALF_ROBOT_SIZE = Units.inchesToMeters(18); - public static final double L4_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15 - 3) ; - public static final double L4_DISTANCE_SCORE = L4_DISTANCE_PREP - Units.inchesToMeters(3); + public static final double L4_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15 - 6); + public static final double L4_DISTANCE_SCORE = L4_DISTANCE_PREP; // public static final double L4_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(4.5); public static final double L3_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15); @@ -114,7 +114,7 @@ public final class Constants { public static final String LEFT_CAMERA_NAME = "CAMERA_LEFT"; public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT"; - public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(5.3), Units.inchesToMeters(8.75), Units.inchesToMeters(9)), new Rotation3d(0,0.0,-15.0 * Math.PI / 180)); + public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(5.3), Units.inchesToMeters(8.75+3), Units.inchesToMeters(9)), new Rotation3d(0,0.0,-15.0 * Math.PI / 180)); public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(0), -Units.inchesToMeters(0), Units.inchesToMeters(0)), new Rotation3d(0,0.0,0)); public static final double MIN_ESTIMATION_DISTANCE = 2; // Meters diff --git a/src/main/java/frc4388/utility/compute/ReefPositionHelper.java b/src/main/java/frc4388/utility/compute/ReefPositionHelper.java index 73dc6a5..5b409e8 100644 --- a/src/main/java/frc4388/utility/compute/ReefPositionHelper.java +++ b/src/main/java/frc4388/utility/compute/ReefPositionHelper.java @@ -55,7 +55,7 @@ public class ReefPositionHelper { } } - System.out.println(minPos.getRotation().getDegrees()); + // System.out.println(minPos.getRotation().getDegrees()); return minPos; }