Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 00:38:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

AUTO rotate

Browse Source
This commit is contained in:
C4llSqin
2026-01-13 19:41:42 -07:00
parent 24fdd610c9
commit a27cde3f84
3 changed files with 65 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+33 -7
View File
@@ -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()));
}
/**
src/main/java/frc4388/robot/constants/BuildConstants.java
+5 -5
View File
@@ -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(){}
src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
+27
View File
@@ -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);