From d437461ec4cc558678b4a78a070424d1aab84acb Mon Sep 17 00:00:00 2001 From: Zach Wilke <83995467+C4llSqin@users.noreply.github.com> Date: Tue, 30 Sep 2025 17:51:59 -0600 Subject: [PATCH] Attempt to calibrate --- .../java/frc4388/robot/RobotContainer.java | 30 +++++++++---------- src/main/java/frc4388/robot/RobotMap.java | 8 ++--- .../robot/constants/BuildConstants.java | 10 +++---- .../frc4388/robot/constants/Constants.java | 6 ++-- .../robot/subsystems/swerve/SwerveDrive.java | 2 +- .../swerve/SwerveDriveConstants.java | 3 +- 6 files changed, 30 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1b6adeb..af0efd4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -79,12 +79,12 @@ 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); - public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef"); + // public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef"); public final LiDAR reverseLidar = new LiDAR(m_robotMap.reverseLidar, "Reverse"); @@ -347,7 +347,7 @@ public class RobotContainer { new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, reefLidar), + // new LidarAlign(m_robotSwerveDrive, reefLidar), // waitDebuger.asProxy(), // new ParallelRaceGroup( // new WaitCommand(1), @@ -388,8 +388,8 @@ public class RobotContainer { new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE-Units.inchesToMeters(1), Side.LEFT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, reefLidar), - waitDebuger.asProxy(), + // new LidarAlign(m_robotSwerveDrive, reefLidar), + // waitDebuger.asProxy(), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -417,8 +417,8 @@ public class RobotContainer { new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, reefLidar), - waitDebuger.asProxy(), + // new LidarAlign(m_robotSwerveDrive, reefLidar), + // waitDebuger.asProxy(), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), @@ -437,8 +437,8 @@ public class RobotContainer { new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, reefLidar), - waitDebuger.asProxy(), + // new LidarAlign(m_robotSwerveDrive, reefLidar), + // waitDebuger.asProxy(), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -458,8 +458,8 @@ public class RobotContainer { new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, reefLidar), - waitDebuger.asProxy(), + // new LidarAlign(m_robotSwerveDrive, reefLidar), + // waitDebuger.asProxy(), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -735,7 +735,7 @@ public class RobotContainer { .onTrue(thrustIntake.asProxy()); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, reefLidar)); + .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive)); // ? /* Operator Buttons */ @@ -748,10 +748,10 @@ public class RobotContainer { // Button box new JoystickButton(getButtonBox(), ButtonBox.Five) - .onTrue(AprilLidarAlignL4LeftSemiAuto); + .onTrue(AprilLidarAlignL4LeftFullAuto); new JoystickButton(getButtonBox(), ButtonBox.One) - .onTrue(AprilLidarAlignL4RightSemiAuto); + .onTrue(AprilLidarAlignL4RightFullAuto); new JoystickButton(getButtonBox(), ButtonBox.Six) .onTrue(AprilLidarAlignL3Left); @@ -813,7 +813,7 @@ public class RobotContainer { .onTrue(AprilLidarAlignL4LeftFullAuto); new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.8 && !operatorManualMode) - .onTrue(AprilLidarAlignL4RightSemiAuto); + .onTrue(AprilLidarAlignL4RightFullAuto); new Trigger(() -> getDeadbandedOperatorController().getLeftBumperButton() && !operatorManualMode) .onTrue(AprilLidarAlignL3Left); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 1951930..16c4423 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,10 +67,10 @@ 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"); @@ -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 f2695bd..2beeb36 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 = 176; - public static final String GIT_SHA = "17239b8920efd45bc5a3b6a098baf4e5509fb558"; - public static final String GIT_DATE = "2025-09-13 11:20:40 MDT"; + public static final int GIT_REVISION = 177; + public static final String GIT_SHA = "141a9eebc14b755ea26b8215c3242834b4ceccbf"; + public static final String GIT_DATE = "2025-09-13 14:26:35 MDT"; public static final String GIT_BRANCH = "advantagekit"; - public static final String BUILD_DATE = "2025-09-13 14:19:44 MDT"; - public static final long BUILD_UNIX_TIME = 1757794784689L; + public static final String BUILD_DATE = "2025-09-30 17:32:30 MDT"; + public static final long BUILD_UNIX_TIME = 1759275150367L; 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 2b6f417..99720f0 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -80,7 +80,7 @@ public final class Constants { public static final double HALF_ROBOT_SIZE = Units.inchesToMeters(18); 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 = L4_DISTANCE_PREP + Units.inchesToMeters(-6); // 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,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(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 Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(5.3), Units.inchesToMeters(8.75+6), 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(5.3), -Units.inchesToMeters(8.75-3), Units.inchesToMeters(9)), new Rotation3d(0,0.0,15.0 * Math.PI / 180)); public static final double MIN_ESTIMATION_DISTANCE = 2; // Meters diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index bbc61e4..df29015 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -277,7 +277,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { .withVelocityY(0) .withTargetDirection(Rotation2d.fromDegrees(angle))); - if (Math.abs(angle - getGyroAngle()) < 5.0) { + if (Math.abs(angle - getGyroAngle()) < 2.0) { return true; } diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index e504379..3fcc3fb 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -98,9 +98,10 @@ public final class SwerveDriveConstants { private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); // -0.502686 -0.426025 + // //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625 - (0.502686 - 0.426025)); + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625 - (0.502686 - 0.426025) -(3.002686 - 2.629395)); 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;