diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 828ed46..2377656 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.utility.CanDevice; import frc4388.utility.DeferredBlock; +import frc4388.utility.DeferredBlockMulti; import frc4388.utility.RobotTime; import frc4388.utility.Status; import frc4388.utility.Subsystem; @@ -113,6 +114,7 @@ public class Robot extends TimedRobot { @Override public void disabledExit() { DeferredBlock.execute(); + DeferredBlockMulti.execute(); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5ba5777..10d04b7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -65,8 +65,10 @@ import frc4388.robot.subsystems.SwerveDrive; // Utilites import frc4388.utility.DeferredBlock; +import frc4388.utility.DeferredBlockMulti; import frc4388.utility.ReefPositionHelper; import frc4388.utility.Subsystem; +import frc4388.utility.TimesNegativeOne; import frc4388.utility.ReefPositionHelper.Side; import frc4388.utility.configurable.ConfigurableString; @@ -324,7 +326,12 @@ public class RobotContainer { configureButtonBindings(); configureVirtualButtonBindings(); - new DeferredBlock(() -> m_robotSwerveDrive.resetGyro()); + new DeferredBlock(() -> { // Called on first robot enable + m_robotSwerveDrive.resetGyro(); + }); + new DeferredBlockMulti(() -> { // Called on every robot enable + TimesNegativeOne.update(); + }); DriverStation.silenceJoystickConnectionWarning(true); // CameraServer.startAutomaticCapture(); @@ -498,7 +505,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..b9fd1be 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,19 +76,26 @@ 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.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis); + // xerr = targetpos.getX() - vision.getPose2d().getX(); + // yerr = targetpos.getX() - vision.getPose2d().getY(); + + // roterr = TimesNegativeOne.invert(targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(), TimesNegativeOne.isRed); + + roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees())); + + boolean invert = Math.abs(roterr) > 180; + + if(roterr > 180){ + roterr -= 360; + }else if(roterr < -180){ + roterr += 360; } - - roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(); + SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees()); + SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees()); + SmartDashboard.putNumber("Rotational PID error", roterr); // SmartDashboard.putNumber("PID X Error", xerr); // SmartDashboard.putNumber("PID Y Error", yerr); diff --git a/src/main/java/frc4388/robot/commands/LidarAlign.java b/src/main/java/frc4388/robot/commands/LidarAlign.java index e25c374..4ae9ad4 100644 --- a/src/main/java/frc4388/robot/commands/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/LidarAlign.java @@ -21,6 +21,7 @@ public class LidarAlign extends Command { private boolean foundReef; private boolean headedRight; private double speed; + private int bounces; // private final boolean constructedHeadedRight; /** Creates a new LidarAlign. */ @@ -40,6 +41,7 @@ public class LidarAlign extends Command { this.speed = 0.4; // TODO: find good speed for this this.foundReef = false; this.headedRight = !(GotoLastApril.tagRelativeXError < 0); + this.bounces = 0; } @@ -55,6 +57,8 @@ public class LidarAlign extends Command { if (currentFinderTick > 10) { //arbutrary threshhold for now. headedRight = !headedRight; currentFinderTick *= -1; + bounces++; + if (bounces == 2) return; } double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI; double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef @@ -78,7 +82,10 @@ public class LidarAlign extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - if (foundReef && lidar.withinDistance()) { // spot on + if (lidar.getDistance() < 4) { + swerveDrive.stopModules(); + return true; + } else if (foundReef && lidar.withinDistance()) { // spot on swerveDrive.stopModules(); return true; } else if (foundReef && !lidar.withinDistance()) { // over shot @@ -87,6 +94,9 @@ public class LidarAlign extends Command { currentFinderTick = 0; foundReef = false; return false; + } else if (bounces == 2) { + swerveDrive.stopModules(); + return true; } else { return false; } diff --git a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java b/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java index 88166a5..fdbb619 100644 --- a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java +++ b/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java @@ -5,6 +5,7 @@ import java.time.Instant; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.TimesNegativeOne; // Command to repeat a joystick movement for a specific time. public class MoveForTimeCommand extends Command { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 1259be0..eff4e3c 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 ); @@ -140,23 +139,14 @@ 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)); - - 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 = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); stopped = false; if (fieldRelative) { + + leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); + rightStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis); + // ! drift correction if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) { rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees(); @@ -218,7 +208,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) @@ -282,7 +272,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/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/DeferredBlockMulti.java b/src/main/java/frc4388/utility/DeferredBlockMulti.java new file mode 100644 index 0000000..9e8b284 --- /dev/null +++ b/src/main/java/frc4388/utility/DeferredBlockMulti.java @@ -0,0 +1,18 @@ +package frc4388.utility; + +import java.util.ArrayList; + +public class DeferredBlockMulti { + private static ArrayList m_blocks = new ArrayList<>(); + + public DeferredBlockMulti(Runnable block) { + m_blocks.add(block); + } + + public static void execute() { + + for (Runnable block : m_blocks) { + block.run(); + } + } +} 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/SlowPeriod.java b/src/main/java/frc4388/utility/SlowPeriod.java deleted file mode 100644 index e872708..0000000 --- a/src/main/java/frc4388/utility/SlowPeriod.java +++ /dev/null @@ -1,6 +0,0 @@ -package frc4388.utility; - -// A very stupid class to hold the state of the swerve speed setting for a short period of time -public class SlowPeriod { - -} diff --git a/src/main/java/frc4388/utility/TimesNegativeOne.java b/src/main/java/frc4388/utility/TimesNegativeOne.java new file mode 100644 index 0000000..bf9818f --- /dev/null +++ b/src/main/java/frc4388/utility/TimesNegativeOne.java @@ -0,0 +1,51 @@ +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; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +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; + public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET); + + 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; + ForwardOffset = Rotation2d.fromDegrees((SwerveDriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0))); + SmartDashboard.putBoolean("Is red alliance", isRed); + } + + 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) + ); + } +} diff --git a/vendordeps/PathplannerLib-2025.1.1.json b/vendordeps/PathplannerLib-2025.2.4.json similarity index 87% rename from vendordeps/PathplannerLib-2025.1.1.json rename to vendordeps/PathplannerLib-2025.2.4.json index 6322388..24add57 100644 --- a/vendordeps/PathplannerLib-2025.1.1.json +++ b/vendordeps/PathplannerLib-2025.2.4.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.1.1.json", + "fileName": "PathplannerLib-2025.2.4.json", "name": "PathplannerLib", - "version": "2025.1.1", + "version": "2025.2.4", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.1.1" + "version": "2025.2.4" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.1.1", + "version": "2025.2.4", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false,