diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 228f61a..5e81e40 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -8,6 +8,8 @@ package frc4388.robot; import java.io.File; + +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; @@ -104,11 +106,24 @@ public class RobotContainer { DriverStation.silenceJoystickConnectionWarning(true); - m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), - // m_robotSwerveDrive.driveWithInput(new Translation2d(.4, 0), - getDeadbandedDriverController().getRight(), - true); + m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + + // 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( + getDeadbandedDriverController().getLeft(), + ang); + } else { + // Drive normally + m_robotSwerveDrive.driveWithInput( + getDeadbandedDriverController().getLeft(), + getDeadbandedDriverController().getRight(),true); + } + }, m_robotSwerveDrive) .withName("SwerveDrive DefaultCommand")); m_robotSwerveDrive.setToSlow(); @@ -131,13 +146,24 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .onTrue(new RotTo45(m_robotSwerveDrive)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + // .onTrue(new RotTo45(m_robotSwerveDrive)); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive)); + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.deactivateLuigiMode())); } /** diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 42495b3..12dd413 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 = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 3; - public static final String GIT_SHA = "8dbb9d5a9099f417617ec2245e275a42b6788116"; - public static final String GIT_DATE = "2026-01-10 16:52:43 MST"; + public static final int GIT_REVISION = 7; + public static final String GIT_SHA = "24fdd610c9256b4599b7d29ebb1a3acd14cc38b7"; + public static final String GIT_DATE = "2026-01-13 18:19:47 MST"; public static final String GIT_BRANCH = "master"; - public static final String BUILD_DATE = "2026-01-12 16:53:00 MST"; - public static final long BUILD_UNIX_TIME = 1768261980081L; + public static final String BUILD_DATE = "2026-01-13 18:58:23 MST"; + public static final long BUILD_UNIX_TIME = 1768355903336L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 2fc6306..a927fe6 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -236,6 +236,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } public void driveRelativeAngle(Translation2d leftStick, Rotation2d heading) { + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); var ctrl = new SwerveRequest.FieldCentricFacingAngle() @@ -250,6 +251,32 @@ public class SwerveDrive extends SubsystemBase implements Queryable { io.setControl(ctrl); } + 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); + + + var ctrl = new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * speedAdjust) + .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 + ); + io.setControl(ctrl); + // SmartDashboard.putBoolean("drift correction", true); + + } + public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) { leftStick = leftStick.rotateBy(heading);