From 93ba351a198d2f83e0592eed3444465fc5b408f6 Mon Sep 17 00:00:00 2001 From: Zach Wilke <83995467+C4llSqin@users.noreply.github.com> Date: Thu, 11 Sep 2025 17:20:49 -0600 Subject: [PATCH] Tune --- .../java/frc4388/robot/RobotContainer.java | 89 ++++++++++++------- src/main/java/frc4388/robot/RobotMap.java | 10 +-- .../robot/constants/BuildConstants.java | 10 +-- .../frc4388/robot/constants/Constants.java | 11 +-- .../swerve/SwerveDriveConstants.java | 2 +- 5 files changed, 74 insertions(+), 48 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index dfbd494..0100142 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -79,7 +79,7 @@ public class RobotContainer { /* Subsystems */ public final LED m_robotLED = new LED(); - public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); + public final Vision m_vision = new Vision(m_robotMap.leftCamera);// m_robotMap.rightCamera); public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); @@ -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::getYButtonPressed); + private Command waitDebuger = new waitSupplier(m_driverXbox::getXButtonPressed); // 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, @@ -120,10 +120,10 @@ public class RobotContainer { private Command AprilLidarAlignL4RightFullAuto = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), - 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.RIGHT, true) - ), () -> m_robotElevator.isL4Primed()), + // 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.RIGHT, true), + // ), () -> m_robotElevator.isL4Primed()), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT), @@ -133,7 +133,7 @@ public class RobotContainer { new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, reefLidar), + // new LidarAlign(m_robotSwerveDrive, reefLidar), waitDebuger.asProxy(), new ParallelRaceGroup( new WaitCommand(1), @@ -205,6 +205,15 @@ public class RobotContainer { new InstantCommand(m_robotSwerveDrive::startSlowPeriod, m_robotSwerveDrive) ); + private Command TempGoToReef = new SequentialCommandGroup( + new InstantCommand(m_robotSwerveDrive::startSlowPeriod, m_robotSwerveDrive), + + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.LEFT, true), + + new InstantCommand(m_robotSwerveDrive::startSlowPeriod, m_robotSwerveDrive) + ); + + /* private Command AprilLidarAlignL4Right = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), @@ -283,41 +292,52 @@ public class RobotContainer { // 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) - ), () -> m_robotElevator.isL4Primed()), + // 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), + // ), () -> 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 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 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 InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), + waitDebuger.asProxy(), + + new waitEndefectorRefrence(m_robotElevator), - - new MoveForTimeCommand(m_robotSwerveDrive, - new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), + waitDebuger.asProxy(), - // // new ConditionalCommand( - // // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + + + // 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()), @@ -671,6 +691,11 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + .onTrue(AprilLidarAlignL4LeftFullAuto); + + // ! /* Speed */ new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); @@ -717,10 +742,10 @@ public class RobotContainer { ), m_robotSwerveDrive)) .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - // .onTrue( new DriveUntilLiDAR(m_robotSwerveDrive, - // new Translation2d(-1,0), new Translation2d(), m_robotMap.reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true)); - .onTrue(WannaSeeMeDunk.asProxy()); + // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + // // .onTrue( new DriveUntilLiDAR(m_robotSwerveDrive, + // // new Translation2d(-1,0), new Translation2d(), m_robotMap.reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true)); + // .onTrue(WannaSeeMeDunk.asProxy()); new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) .onTrue(thrustIntake.asProxy()); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 01cdb68..1951930 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -45,7 +45,7 @@ public class RobotMap { // public RobotGyro gyro = new RobotGyro(m_pigeon2); public final VisionIO leftCamera; - public final VisionIO rightCamera; + // public final VisionIO rightCamera; // public final LiDAR lidar = new @@ -67,13 +67,13 @@ public class RobotMap { case REAL: // Configure cameras PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); - PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); + // PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); leftCamera = new VisionReal(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ; - rightCamera = new VisionReal(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS); + // rightCamera = new VisionReal(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS); FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera"); - FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera"); + // FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera"); // Configure LiDAR reefLidar = new LidarReal(LiDARConstants.REEF_LIDAR_DIO_CHANNEL); @@ -126,7 +126,7 @@ public class RobotMap { // break; default: leftCamera = new VisionIO() {}; - rightCamera = new VisionIO() {}; + // rightCamera = new VisionIO() {}; reefLidar = new LidarIO() {}; reverseLidar = new LidarIO() {}; swerveDrivetrain = new SwerveIO() {}; diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 5aeda6b..3d8da9b 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 = 173; - public static final String GIT_SHA = "2b7bb1224195b9e001e60b895ee04d63abdfc513"; - public static final String GIT_DATE = "2025-07-17 09:15:19 MDT"; + public static final int GIT_REVISION = 174; + public static final String GIT_SHA = "44286f81e7da4b05dddce68cf50b3d6daf12282a"; + public static final String GIT_DATE = "2025-07-17 21:05:58 MDT"; public static final String GIT_BRANCH = "advantagekit"; - public static final String BUILD_DATE = "2025-07-17 11:55:31 MDT"; - public static final long BUILD_UNIX_TIME = 1752774931371L; + public static final String BUILD_DATE = "2025-09-09 17:15:09 MDT"; + public static final long BUILD_UNIX_TIME = 1757459709649L; 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 6e8733d..a993e44 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -49,7 +49,8 @@ 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(8,0,0.0); + public static final Gains XY_GAINS = new Gains(0.8,0,0.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); @@ -78,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); - public static final double L4_DISTANCE_SCORE = L4_DISTANCE_PREP; + 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_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(4.5); public static final double L3_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15); @@ -113,8 +114,8 @@ 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(4.547), Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0)); - public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0)); + 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 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/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index 35b02db..c832ef6 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -98,7 +98,7 @@ public final class SwerveDriveConstants { private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625+0.5); + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625); private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;