diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index bd1a358..80bcf5e 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -264,6 +264,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { 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 + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); @@ -285,6 +286,18 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // SmartDashboard.putBoolean("drift correction", true); } + + public void driveIntake(Translation2d leftStick, Rotation2d heading, boolean invert){ + if (invert){ + Translation2d stick = new Translation2d(-leftStick.getX(), -leftStick.getY()); + driveFieldAngle(stick, heading); + + } else{ + driveFieldAngle(leftStick, heading); + } + } + + // Drive with the robot facing towards a specific position public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos) {