diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 00e4f50..c475ab0 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -65,6 +65,7 @@ import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.DeferredBlock; import frc4388.utility.ReefPositionHelper; import frc4388.utility.Subsystem; +import frc4388.utility.TimesNegativeOne; import frc4388.utility.ReefPositionHelper.Side; import frc4388.utility.configurable.ConfigurableString; @@ -292,7 +293,10 @@ public class RobotContainer { configureButtonBindings(); configureVirtualButtonBindings(); - new DeferredBlock(() -> m_robotSwerveDrive.resetGyro()); + new DeferredBlock(() -> { // Called on robot enable + TimesNegativeOne.update(); + m_robotSwerveDrive.resetGyro(); + }); DriverStation.silenceJoystickConnectionWarning(true); // CameraServer.startAutomaticCapture(); @@ -462,7 +466,6 @@ public class RobotContainer { new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180) .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepDown())); - new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90) .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepUp())); diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java index b02c209..1e4a5d3 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/src/main/java/frc4388/robot/commands/GotoLastApril.java @@ -15,6 +15,7 @@ import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Vision; import frc4388.utility.Gains; import frc4388.utility.ReefPositionHelper; +import frc4388.utility.TimesNegativeOne; import frc4388.utility.ReefPositionHelper.Side; import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; import frc4388.utility.UtilityStructs.AutoRecordingFrame; @@ -75,18 +76,9 @@ public class GotoLastApril extends Command { @Override public void execute() { - - if (alliance == Alliance.Red) { - xerr = -(targetpos.getX() - vision.getPose2d().getX()); - yerr = (targetpos.getY() - vision.getPose2d().getY()); - } - else{ - - xerr = targetpos.getX() - vision.getPose2d().getX(); - yerr = targetpos.getY() - vision.getPose2d().getY(); - } + xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis); + yerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.YAxis); - roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(); // SmartDashboard.putNumber("PID X Error", xerr); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 1221930..cfb3afa 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -35,9 +35,8 @@ import frc4388.robot.commands.GotoLastApril; import frc4388.robot.commands.LidarAlign; import frc4388.utility.Status; import frc4388.utility.Subsystem; +import frc4388.utility.TimesNegativeOne; import frc4388.utility.Status.ReportLevel; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.config.PIDConstants; @@ -103,11 +102,11 @@ public class SwerveDrive extends Subsystem { // This will flip the path being followed to the red side of the field. // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; + // var alliance = DriverStation.getAlliance(); + // if (alliance.isPresent()) { + // return alliance.get() == DriverStation.Alliance.Red; + // } + return TimesNegativeOne.isRed; }, this // Reference to this subsystem to set requirements ); @@ -141,18 +140,8 @@ public class SwerveDrive extends Subsystem { return; // don't bother doing swerve drive math and return early. leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); - - Optional alliance = DriverStation.getAlliance(); - - if(!alliance.isEmpty()){ - if (alliance.get() == Alliance.Red) - leftStick = new Translation2d(-leftStick.getX(), leftStick.getY()); - else - leftStick = new Translation2d(leftStick.getX(), -leftStick.getY()); - // if (alliance.get() != Alliance.Red) leftStick = new Translation2d(leftStick.getX(), -leftStick.getY()); - } - if (SwerveDriveConstants.INVERT_ROTATION) rightStick.times(-1); - + leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); + leftStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis); stopped = false; diff --git a/src/main/java/frc4388/utility/Alliance.java b/src/main/java/frc4388/utility/Alliance.java new file mode 100644 index 0000000..0f96914 --- /dev/null +++ b/src/main/java/frc4388/utility/Alliance.java @@ -0,0 +1,5 @@ +package frc4388.utility; + +public class Alliance { + +} diff --git a/src/main/java/frc4388/utility/ReefPositionHelper.java b/src/main/java/frc4388/utility/ReefPositionHelper.java index d063568..e8cb0f2 100644 --- a/src/main/java/frc4388/utility/ReefPositionHelper.java +++ b/src/main/java/frc4388/utility/ReefPositionHelper.java @@ -63,14 +63,11 @@ public class ReefPositionHelper { * Function to find closest tag location based on side */ public static Pose2d getNearestTag(Pose2d position) { - Optional ally = DriverStation.getAlliance(); - if (!ally.isPresent()) - return new Pose2d(); - if (ally.get() == Alliance.Red) + + if(TimesNegativeOne.isRed) return getNearestTag(RED_TAGS, position); - if (ally.get() == Alliance.Blue) - return getNearestTag(BLUE_TAGS, position); - return new Pose2d(); + else + return getNearestTag(BLUE_TAGS, position); } public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim, double ydistance) { diff --git a/src/main/java/frc4388/utility/TimesNegativeOne.java b/src/main/java/frc4388/utility/TimesNegativeOne.java new file mode 100644 index 0000000..5887114 --- /dev/null +++ b/src/main/java/frc4388/utility/TimesNegativeOne.java @@ -0,0 +1,46 @@ +package frc4388.utility; + +import java.util.Optional; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc4388.robot.Constants.SwerveDriveConstants; + +// Class that holds weather the drivers sticks should be inverted +public class TimesNegativeOne { + + public static boolean XAxis = SwerveDriveConstants.INVERT_X; + public static boolean YAxis = SwerveDriveConstants.INVERT_Y; + public static boolean RotAxis = SwerveDriveConstants.INVERT_ROTATION; + public static boolean isRed = false; + + private static boolean isRed() { + Optional alliance = DriverStation.getAlliance(); + if(alliance.isEmpty()) return false; + return (alliance.get() == Alliance.Red); + } + + public static void update(){ + isRed = isRed(); + XAxis = SwerveDriveConstants.INVERT_X ^ isRed; + YAxis = SwerveDriveConstants.INVERT_Y ^ isRed; + RotAxis = SwerveDriveConstants.INVERT_ROTATION; + } + + public static double invert(double num, boolean invert){ + return invert ? -num : num; + } + + public static Translation2d invert(Translation2d stick, boolean invertXY){ + if(invertXY) return stick.times(-1); + else return stick; + } + + public static Translation2d invert(Translation2d stick, boolean invertX, boolean invertY){ + return new Translation2d( + invert(stick.getX(), invertX), + invert(stick.getY(), invertY) + ); + } +}