From 5f9a38f55d5774614469df958ae92836bd06060a Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Wed, 15 Jan 2025 16:32:13 -0700 Subject: [PATCH] 2024x2025 --- src/main/java/frc4388/robot/Constants.java | 9 +++++++++ src/main/java/frc4388/robot/RobotContainer.java | 12 ++++++------ .../java/frc4388/robot/subsystems/SwerveDrive.java | 2 +- src/main/java/frc4388/robot/subsystems/Vision.java | 6 +++++- 4 files changed, 21 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 0108060..f036098 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -99,40 +99,49 @@ public final class Constants { public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084; public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; + // public static final double FORWARD_OFFSET = 270; // 0, 90, 180, 270 public static final double FORWARD_OFFSET = 0; // 0, 90, 180, 270 public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 1.d, 10); private static final class ModuleSpecificConstants { //Front Left + // private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125); private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.229736328125+.25); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; + // private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH); private static final Distance FRONT_LEFT_XPOS = Inches.of(-HALF_WIDTH); private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Front Right + // private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.133056640625); private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.371337890625+.5+.25); private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH); + // private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); private static final Distance FRONT_RIGHT_YPOS = Inches.of(HALF_HEIGHT); //Back Left + // private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.47705078125 + 0.5); private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.3828125+.25+.5); private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH); + // private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); private static final Distance BACK_LEFT_YPOS = Inches.of(-HALF_HEIGHT); //Back Right + // private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.355224609375 + 0.5); private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.01904296875+.25); 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; + // private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH); private static final Distance BACK_RIGHT_XPOS = Inches.of(HALF_WIDTH); private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b127f61..9aa79d9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -155,14 +155,14 @@ public class RobotContainer { // @ /* Trim Test Buttons */ - new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepUp())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + // .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepUp())); - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepDown())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + // .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.stepDown())); - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.load())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + // .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.load())); // ! /* Speed */ new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 14bf6a8..2ae0061 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -181,7 +181,7 @@ public class SwerveDrive extends Subsystem { } public void stopModules() { - // swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); + swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); } @Override diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 07ca483..d121854 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -3,6 +3,7 @@ package frc4388.robot.subsystems; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.numbers.N1; @@ -73,6 +74,9 @@ public class Vision extends Subsystem { @Override public void periodic() { var result = camera.getLatestResult(); + // var results = camera.getAllUnreadResults(); + // if (results.size() == 0) return; + // var result = results.get(results.size()-1); isTag = result.hasTargets(); // Optional multitag = result.getMultiTagResult(); @@ -106,7 +110,7 @@ public class Vision extends Subsystem { } lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d(); - // lastVisionPose.rotateBy(lastVisionPose.getRotation().minus(lastPhysOdomPose.getRotation())); + lastVisionPose.rotateBy(Rotation2d.k180deg); // lastVisionPose = new Pose2d( // lastVisionPose.getTranslation(), // lastPhysOdomPose.getRotation()