diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index cfb3afa..bb2b05c 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -139,7 +139,7 @@ public class SwerveDrive extends Subsystem { if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput return; // don't bother doing swerve drive math and return early. - leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); leftStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis); @@ -207,7 +207,7 @@ public class SwerveDrive extends Subsystem { if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput return; // don't bother doing swerve drive math and return early. - leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + leftStick.rotateBy(TimesNegativeOne.ForwardOffset); swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(leftStick.getX() * speedAdjust) @@ -271,7 +271,7 @@ public class SwerveDrive extends Subsystem { // if (leftStick.getNorm() < 0.05) //if no imput // return; // don't bother doing swerve drive math and return early. - leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(leftStick.getX() * -speedAdjust) diff --git a/src/main/java/frc4388/utility/TimesNegativeOne.java b/src/main/java/frc4388/utility/TimesNegativeOne.java index 5887114..1846b5e 100644 --- a/src/main/java/frc4388/utility/TimesNegativeOne.java +++ b/src/main/java/frc4388/utility/TimesNegativeOne.java @@ -2,6 +2,7 @@ package frc4388.utility; import java.util.Optional; +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.DriverStation.Alliance; @@ -14,6 +15,7 @@ public class TimesNegativeOne { public static boolean YAxis = SwerveDriveConstants.INVERT_Y; public static boolean RotAxis = SwerveDriveConstants.INVERT_ROTATION; public static boolean isRed = false; + public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET); private static boolean isRed() { Optional alliance = DriverStation.getAlliance(); @@ -23,9 +25,10 @@ public class TimesNegativeOne { public static void update(){ isRed = isRed(); - XAxis = SwerveDriveConstants.INVERT_X ^ isRed; - YAxis = SwerveDriveConstants.INVERT_Y ^ isRed; + XAxis = SwerveDriveConstants.INVERT_X;// ^ isRed; + YAxis = SwerveDriveConstants.INVERT_Y;// ^ isRed; RotAxis = SwerveDriveConstants.INVERT_ROTATION; + ForwardOffset = Rotation2d.fromDegrees((SwerveDriveConstants.FORWARD_OFFSET + (isRed ? 180 : 0))); } public static double invert(double num, boolean invert){