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,