From c68329c9cac4515e1f14398d9ddab5a08b903db5 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Sun, 18 Jan 2026 12:53:07 -0700 Subject: [PATCH] Add aim lead --- .../java/frc4388/robot/RobotContainer.java | 8 ++--- .../frc4388/robot/constants/Constants.java | 13 -------- .../robot/constants/FieldConstants.java | 14 ++++++++ .../robot/subsystems/swerve/SwerveDrive.java | 33 ++++++++++++++----- .../swerve/SwerveDriveConstants.java | 12 +++++++ .../robot/subsystems/vision/VisionReal.java | 2 +- 6 files changed, 55 insertions(+), 27 deletions(-) create mode 100644 src/main/java/frc4388/robot/constants/FieldConstants.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5e81e40..ded3270 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -39,6 +39,7 @@ import frc4388.robot.commands.MoveUntilSuply; // import frc4388.robot.commands.wait.waitFeedCoral; import frc4388.robot.commands.wait.waitSupplier; import frc4388.robot.constants.Constants; +import frc4388.robot.constants.FieldConstants; import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; @@ -111,12 +112,9 @@ public class RobotContainer { // IF the driver is holding the aim button, aim the robot towards the hub if(m_driverXbox.getRightTriggerAxis() > 0.5) { // Aim - Translation2d shootTarget = new Translation2d(); - // new Rotation2() - Rotation2d ang = m_robotSwerveDrive.getPose2d().getTranslation().minus(shootTarget).getAngle(); - m_robotSwerveDrive.driveFieldAngle( + m_robotSwerveDrive.driveFacingPosition( getDeadbandedDriverController().getLeft(), - ang); + FieldConstants.BLUE_HUB_POS); } else { // Drive normally m_robotSwerveDrive.driveWithInput( diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index f9c9594..dd7b8af 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -92,19 +92,6 @@ public final class Constants { public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.1, 0.1, 1); } - public static final class FieldConstants { - public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); - - // Test april tag field layout - // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( - // Arrays.asList(new AprilTag[] { - // new AprilTag(1, new Pose3d( - // new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.) - // )), - // }), 100, 100); - - } - public static final class LEDConstants { public static final int LED_SPARK_ID = 9; diff --git a/src/main/java/frc4388/robot/constants/FieldConstants.java b/src/main/java/frc4388/robot/constants/FieldConstants.java new file mode 100644 index 0000000..ab79e79 --- /dev/null +++ b/src/main/java/frc4388/robot/constants/FieldConstants.java @@ -0,0 +1,14 @@ +package frc4388.robot.constants; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Translation2d; + +public final class FieldConstants { + public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + + public static final Translation2d BLUE_HUB_POS = new Translation2d(); + public static final Translation2d RED_HUB_POS = new Translation2d(); + + +} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index a927fe6..3ec59a0 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -251,14 +251,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable { io.setControl(ctrl); } + // Drive with a specific velocity and heading public void driveFieldAngle(Translation2d leftStick, Rotation2d heading) { if (leftStick.getNorm() < 0.05) // if no imput and the swerve drive is still going: stopModules(); // stop the swerve - // if (leftStick.getNorm() < 0.05) // if no imput - // return; // don't bother doing swerve drive math and return early. - - leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); @@ -268,13 +265,33 @@ public class SwerveDrive extends SubsystemBase implements Queryable { .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(heading); ctrl.HeadingController.setPID( - SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP, - SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI, - SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD + SwerveDriveConstants.PIDConstants.AIM_kP.get(), + SwerveDriveConstants.PIDConstants.AIM_kI.get(), + SwerveDriveConstants.PIDConstants.AIM_kD.get() + // SwerveDriveConstants.PIDConstants.AIM_GAINS.kP, + // SwerveDriveConstants.PIDConstants.AIM_GAINS.kI, + // SwerveDriveConstants.PIDConstants.AIM_GAINS.kD ); io.setControl(ctrl); // SmartDashboard.putBoolean("drift correction", true); - + } + + // Drive with the robot facing towards a specific position + public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos) { + + // Get the current speed of the robot + Translation2d robotSpeed = new Translation2d( + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond + ); + + // Calculate a point to aim ahead of the actual position. + Translation2d fieldPosLead = robotSpeed.times(SwerveDriveConstants.AIM_LEAD_TIME.get()).plus(fieldPos); + + // Calculate the angle between the current position and the lead position + Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle(); + + driveFieldAngle(leftStick, ang); } public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) { diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index f3bbe9f..acdf666 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -22,6 +22,7 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Angle; +import frc4388.utility.configurable.ConfigurableDouble; //import edu.wpi.first.units.measure.measure.Distance; import frc4388.utility.status.CanDevice; import frc4388.utility.structs.Gains; @@ -37,6 +38,9 @@ public final class SwerveDriveConstants { public static double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED; public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED; + // TODO: Replace with a constant + public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0); + public static final double CORRECTION_MIN = 10; public static final double CORRECTION_MAX = 50; @@ -156,6 +160,14 @@ public final class SwerveDriveConstants { public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1); public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(10, 0, 1); + + + + // TODO: Replace this with a static constant + public static final ConfigurableDouble AIM_kP = new ConfigurableDouble("Aim kP", 2.5); + public static final ConfigurableDouble AIM_kI = new ConfigurableDouble("Aim kI", 0); + public static final ConfigurableDouble AIM_kD = new ConfigurableDouble("Aim kD", 0.1); + // public static final Gains AIM_GAINS = new Gains(2.5, 0, 0.1); } public static final class Configurations { diff --git a/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java index 1c12437..82fbd1f 100644 --- a/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java +++ b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java @@ -8,7 +8,7 @@ import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.targeting.PhotonPipelineResult; -import frc4388.robot.constants.Constants.FieldConstants; +import frc4388.robot.constants.FieldConstants; import frc4388.robot.constants.Constants.VisionConstants; public class VisionReal implements VisionIO {