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/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);