From 07841b303a96102f277faafee70fab21b95c9f46 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Mon, 24 Feb 2025 17:59:44 -0700 Subject: [PATCH 1/5] x /= i^2 --- .../java/frc4388/robot/RobotContainer.java | 7 ++- .../frc4388/robot/commands/GotoLastApril.java | 14 ++---- .../frc4388/robot/subsystems/SwerveDrive.java | 27 ++++------- src/main/java/frc4388/utility/Alliance.java | 5 ++ .../frc4388/utility/ReefPositionHelper.java | 11 ++--- .../frc4388/utility/TimesNegativeOne.java | 46 +++++++++++++++++++ 6 files changed, 71 insertions(+), 39 deletions(-) create mode 100644 src/main/java/frc4388/utility/Alliance.java create mode 100644 src/main/java/frc4388/utility/TimesNegativeOne.java 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) + ); + } +} From 11861bf5809415e643ea81f2e905f049ed0dc1c4 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Tue, 25 Feb 2025 11:48:39 -0700 Subject: [PATCH 2/5] Use forward offset instead of negative one for swerve axis --- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 6 +++--- src/main/java/frc4388/utility/TimesNegativeOne.java | 7 +++++-- 2 files changed, 8 insertions(+), 5 deletions(-) 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){ From 57a9435c5d380c8cff959ad0dce8d5febb46d65a Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Tue, 25 Feb 2025 19:49:10 -0700 Subject: [PATCH 3/5] fix: excise ballerinas from robot code --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/Robot.java | 2 ++ .../java/frc4388/robot/RobotContainer.java | 7 +++++-- .../frc4388/robot/commands/GotoLastApril.java | 20 +++++++++++++++++-- .../frc4388/robot/commands/LidarAlign.java | 12 ++++++++++- .../robot/commands/MoveForTimeCommand.java | 1 + .../frc4388/robot/subsystems/SwerveDrive.java | 9 +++++---- .../frc4388/utility/DeferredBlockMulti.java | 18 +++++++++++++++++ .../frc4388/utility/TimesNegativeOne.java | 8 +++++--- ....1.1.json => PathplannerLib-2025.2.4.json} | 8 ++++---- 10 files changed, 70 insertions(+), 17 deletions(-) create mode 100644 src/main/java/frc4388/utility/DeferredBlockMulti.java rename vendordeps/{PathplannerLib-2025.1.1.json => PathplannerLib-2025.2.4.json} (87%) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 6e73085..cab9b1f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -366,7 +366,7 @@ public final class Constants { // public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right - public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(17); + public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(16); public static final double L4_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); public static final double L4_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); public static final double L3_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); 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 c475ab0..55feb98 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -63,6 +63,7 @@ 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; @@ -293,10 +294,12 @@ public class RobotContainer { configureButtonBindings(); configureVirtualButtonBindings(); - new DeferredBlock(() -> { // Called on robot enable - TimesNegativeOne.update(); + new DeferredBlock(() -> { // Called on first robot enable m_robotSwerveDrive.resetGyro(); }); + new DeferredBlockMulti(() -> { // Called on every robot enable + TimesNegativeOne.update(); + }); DriverStation.silenceJoystickConnectionWarning(true); // CameraServer.startAutomaticCapture(); diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java index 1e4a5d3..b9fd1be 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/src/main/java/frc4388/robot/commands/GotoLastApril.java @@ -77,9 +77,25 @@ public class GotoLastApril extends Command { @Override public void execute() { xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis); - yerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.YAxis); + yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis); + // xerr = targetpos.getX() - vision.getPose2d().getX(); + // yerr = targetpos.getX() - vision.getPose2d().getY(); - roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(); + // 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; + } + + 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 bb2b05c..d580a58 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -139,13 +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(TimesNegativeOne.ForwardOffset); - leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); - leftStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis); - + 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(); 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/TimesNegativeOne.java b/src/main/java/frc4388/utility/TimesNegativeOne.java index 1846b5e..bf9818f 100644 --- a/src/main/java/frc4388/utility/TimesNegativeOne.java +++ b/src/main/java/frc4388/utility/TimesNegativeOne.java @@ -6,6 +6,7 @@ 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 @@ -25,10 +26,11 @@ 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))); + ForwardOffset = Rotation2d.fromDegrees((SwerveDriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0))); + SmartDashboard.putBoolean("Is red alliance", isRed); } public static double invert(double num, boolean invert){ 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, From eb5dce08df02547c53b89b2209e0c814afc22943 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Wed, 26 Feb 2025 15:40:29 -0700 Subject: [PATCH 4/5] Add if command, and speed saving --- .../java/frc4388/robot/RobotContainer.java | 85 +++++++++++++------ .../frc4388/robot/commands/IfCommand.java | 33 +++++++ .../frc4388/robot/subsystems/Elevator.java | 8 ++ .../java/frc4388/robot/subsystems/Lidar.java | 2 +- .../frc4388/robot/subsystems/SwerveDrive.java | 12 +++ src/main/java/frc4388/utility/SlowPeriod.java | 6 ++ 6 files changed, 117 insertions(+), 29 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/IfCommand.java create mode 100644 src/main/java/frc4388/utility/SlowPeriod.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 38291cc..5ba5777 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj.DriverStation; import java.io.File; import java.util.ArrayList; import java.util.List; +import java.util.function.BooleanSupplier; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.geometry.Translation2d; @@ -41,6 +42,7 @@ import edu.wpi.first.wpilibj2.command.Commands; // Autos import frc4388.utility.controller.VirtualController; import frc4388.robot.commands.GotoLastApril; +import frc4388.robot.commands.IfCommand; import frc4388.robot.commands.LidarAlign; import frc4388.robot.commands.MoveForTimeCommand; import frc4388.robot.commands.waitElevatorRefrence; @@ -117,11 +119,15 @@ public class RobotContainer { // true, false); // private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision); private Command AprilLidarAlignL4Right = new SequentialCommandGroup( - new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT) + )), - new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT), + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -136,17 +142,22 @@ public class RobotContainer { new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), 500, true), - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator) + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); private Command AprilLidarAlignL4Left = new SequentialCommandGroup( - new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT) + )), + + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT), - new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT), new waitEndefectorRefrence(m_robotElevator), - new waitElevatorRefrence(m_robotElevator), new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.LEFT), @@ -161,17 +172,21 @@ public class RobotContainer { new Translation2d(0,1), new Translation2d(), 500, true), - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator) + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); private Command AprilLidarAlignL3Left = new SequentialCommandGroup( - new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT), - // new InstantCommand(() -> System.out.println("Soup")), - // new WaitCommand(1), + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + + new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT) + )), + new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.LEFT), new LidarAlign(m_robotSwerveDrive, m_lidar), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), @@ -179,13 +194,19 @@ public class RobotContainer { new waitElevatorRefrence(m_robotElevator), // new MoveForTimeCommand(m_robotSwerveDrive, // new Translation2d(0,1), new Translation2d(), 500, true), - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator) + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); private Command AprilLidarAlignL3Right = new SequentialCommandGroup( - new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + + new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT) + )), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT), new waitEndefectorRefrence(m_robotElevator), @@ -201,11 +222,12 @@ public class RobotContainer { new waitElevatorRefrence(m_robotElevator), // new MoveForTimeCommand(m_robotSwerveDrive, // new Translation2d(0,1), new Translation2d(), 500, true), - new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator) + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); private Command AprilLidarAlignL2Left = new SequentialCommandGroup( - new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.LEFT), new LidarAlign(m_robotSwerveDrive, m_lidar), @@ -214,12 +236,13 @@ public class RobotContainer { new waitElevatorRefrence(m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), 500, true), - new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator) + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); private Command AprilLidarAlignL2Right = new SequentialCommandGroup( - new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L2_SCORE_DISTANCE, Side.RIGHT), new LidarAlign(m_robotSwerveDrive, m_lidar), @@ -228,8 +251,8 @@ public class RobotContainer { new waitElevatorRefrence(m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), 500, true), - new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator) - + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); private Command placeCoral = new SequentialCommandGroup( @@ -251,7 +274,7 @@ public class RobotContainer { ); private Command leftAlgaeRemove = new SequentialCommandGroup( - new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -260,11 +283,12 @@ public class RobotContainer { new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), 500, true), - new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator) + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); private Command rightAlgaeRemove = new SequentialCommandGroup( - new InstantCommand(() -> {m_robotSwerveDrive.shiftDown();m_robotSwerveDrive.shiftDown();}), + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), @@ -273,7 +297,8 @@ public class RobotContainer { new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), 500, true), - new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator) + new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); @@ -449,7 +474,11 @@ public class RobotContainer { // Cancel button new JoystickButton(getButtonBox(), ButtonBox.White) - .onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endefectorStop();}, m_robotElevator)); + .onTrue(new InstantCommand(() -> { + m_robotElevator.elevatorStop(); + m_robotElevator.endefectorStop(); + m_robotSwerveDrive.endSlowPeriod(); + }, m_robotElevator)); // Score prep // Prime 4 diff --git a/src/main/java/frc4388/robot/commands/IfCommand.java b/src/main/java/frc4388/robot/commands/IfCommand.java new file mode 100644 index 0000000..0f5cd47 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/IfCommand.java @@ -0,0 +1,33 @@ +package frc4388.robot.commands; + +import java.time.Instant; +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +// Command that is called if something is true +public class IfCommand extends InstantCommand { + BooleanSupplier isTrue; + Command trueCommand; + Command falseCommand; + + public IfCommand(BooleanSupplier isTrue, Command trueCommand, Command falseCommand){ + this.isTrue = isTrue; + this.trueCommand = trueCommand; + this.falseCommand = falseCommand; + } + + public IfCommand(BooleanSupplier isTrue, Command trueCommand){ + this(isTrue, trueCommand, Commands.none()); + } + + @Override + public void execute() { + if(isTrue.getAsBoolean()) + trueCommand.schedule(); + else + falseCommand.schedule(); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 933a444..5d04769 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -243,6 +243,14 @@ public class Elevator extends SubsystemBase { // } } + public boolean isL4Primed(){ + return currentState == CoordinationState.PrimedFour; + } + + public boolean isL3Primed(){ + return currentState == CoordinationState.PrimedThree; + } + public void armShuffle(){ if(!basinBeamBreak.get()){ //shuffle the coral with the arm until coral hits beam break diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java index 130efc3..5fad561 100644 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ b/src/main/java/frc4388/robot/subsystems/Lidar.java @@ -72,7 +72,7 @@ public class Lidar extends Subsystem { if(distance == -1){ s.addReport(ReportLevel.ERROR, "LIDAR DISCONNECTED"); }else{ - s.addReport(ReportLevel.ERROR, "LIDAR CONNECTED"); + s.addReport(ReportLevel.INFO, "LIDAR CONNECTED"); } return s; } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 1221930..1259be0 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -390,6 +390,18 @@ public class SwerveDrive extends Subsystem { rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; } + private int tmp_gear_index = SwerveDriveConstants.STARTING_GEAR; + + public void startSlowPeriod() { + tmp_gear_index = gear_index; + setToSlow(); + } + + public void endSlowPeriod() { + setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]); + gear_index = tmp_gear_index; + } + @Override public String getSubsystemName() { return "Swerve Drive Controller"; diff --git a/src/main/java/frc4388/utility/SlowPeriod.java b/src/main/java/frc4388/utility/SlowPeriod.java new file mode 100644 index 0000000..e872708 --- /dev/null +++ b/src/main/java/frc4388/utility/SlowPeriod.java @@ -0,0 +1,6 @@ +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 { + +} From 3cf162de7475f45857f037325614b21033113660 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Wed, 26 Feb 2025 16:59:07 -0700 Subject: [PATCH 5/5] Goodbye SlowPeriod! --- src/main/java/frc4388/utility/SlowPeriod.java | 6 ------ 1 file changed, 6 deletions(-) delete mode 100644 src/main/java/frc4388/utility/SlowPeriod.java 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 { - -}