From 3daa1b7c5b475e2a0b74b9550f8215811876a090 Mon Sep 17 00:00:00 2001 From: ThePixelatedCat Date: Sat, 9 Aug 2025 18:15:28 +1000 Subject: [PATCH 01/13] Started de-staticing Limelight --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/Superstructure.java | 33 ++++++++++---- .../java/frc/robot/subsystems/Limelight.java | 44 +++++++++++-------- 3 files changed, 50 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 03bac83..2e47eec 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -29,7 +29,7 @@ public Robot() @Override public void robotPeriodic() { - superstructure.updateSwerveState(); + superstructure.periodic(); CommandScheduler.getInstance().run(); } diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index e77b68b..dbb4377 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -1,5 +1,6 @@ package frc.robot; +import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; @@ -59,6 +60,7 @@ public enum DriveState {Reef, Station, Barge, None} private CoralRoller s_Coral; private Limelight s_foreLL; private Limelight s_aftLL; + private Limelight[] limelights = {s_foreLL, s_aftLL}; private static TargetPosition currentTarget; private static DriveState currentDriveState; @@ -249,19 +251,32 @@ private void bindRumbles() io_copilotRight.addRumbleTrigger("ready to score" , new Trigger(() -> FieldUtils.atReefLineUp(swerveState.Pose.getTranslation()))); } - public Command getAutonomousCommand() + public void periodic() { - return AutoFactories.getCommandList(SD.IO_AUTO.get(), s_Coral, s_Swerve, () -> swerveState); - } - - public static void addVisionMeasurement(Pose2d poseMeasurement, double timestamp) - { - s_Swerve.addVisionMeasurement(poseMeasurement, timestamp); + updateSwerveState(); + + if (SD.IO_LL.get()) + { + for (Limelight ll : limelights) + { + ll.fetchVisionValues().ifPresent + ( + visionVals -> + { + var stdDevs = visionVals.getFirst(); + var mt2 = visionVals.getSecond(); + + s_Swerve.setVisionMeasurementStdDevs(stdDevs); + s_Swerve.addVisionMeasurement(mt2.pose, Utils.fpgaToCurrentTime(mt2.timestampSeconds)); + } + ); + } + } } - public static void setVisionMeasurementStdDevs(Matrix visionMeasurementStdDevs) + public Command getAutonomousCommand() { - s_Swerve.setVisionMeasurementStdDevs(visionMeasurementStdDevs); + return AutoFactories.getCommandList(SD.IO_AUTO.get(), s_Coral, s_Swerve, () -> swerveState); } public double robotRadiusSup() diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index bb5d115..25cc4f6 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -5,19 +5,25 @@ package frc.robot.subsystems; import java.util.ArrayList; +import java.util.Optional; import com.ctre.phoenix6.Utils; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Pair; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Superstructure; import frc.robot.constants.Constants; import frc.robot.util.SD; +import frc.robot.util.LimelightHelpers.PoseEstimate; import frc.robot.util.LimelightHelpers; public class Limelight extends SubsystemBase @@ -109,6 +115,25 @@ public int updateLimelightPipeline() public Pose3d getMT1Pose() {return LimelightHelpers.getBotPose3d_wpiBlue(limelightName);} + public Optional, PoseEstimate>> fetchVisionValues() + { + mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName); + //mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName); + + useUpdate = !(mt2 == null || mt2.tagCount == 0 || omegaRps > 2.0); + + if (useUpdate) + { + stdDevFactor = Math.pow(mt2.avgTagDist, 2.0) / mt2.tagCount; + + linearStdDev = Constants.Vision.linearStdDevBaseline * stdDevFactor; + rotStdDev = Constants.Vision.rotStdDevBaseline * stdDevFactor; + + return Optional.of(Pair.of(VecBuilder.fill(linearStdDev, linearStdDev, rotStdDev), mt2)); + } + return Optional.empty(); + } + @Override public void periodic() { @@ -170,25 +195,6 @@ public void periodic() LimelightHelpers.SetRobotOrientation(limelightName, headingDeg, 0, 0, 0, 0, 0); LimelightHelpers.SetFiducialIDFiltersOverride(limelightName, validIDs); - - if (SD.IO_LL.get()) //TODO: Replace MT1 references with MT2 when available - { - mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName); - //mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName); - - useUpdate = !(mt2 == null || mt2.tagCount == 0 || omegaRps > 2.0); - - if (useUpdate) - { - stdDevFactor = Math.pow(mt2.avgTagDist, 2.0) / mt2.tagCount; - - linearStdDev = Constants.Vision.linearStdDevBaseline * stdDevFactor; - rotStdDev = Constants.Vision.rotStdDevBaseline * stdDevFactor; - - Superstructure.setVisionMeasurementStdDevs(VecBuilder.fill(linearStdDev, linearStdDev, rotStdDev)); - Superstructure.addVisionMeasurement(mt2.pose, Utils.fpgaToCurrentTime(mt2.timestampSeconds)); - } - } SD.SENSOR_GYRO.put(headingDeg); } From b8a64b3524426c7228e430a32ef0cb58b62810ae Mon Sep 17 00:00:00 2001 From: ThePixelatedCat Date: Thu, 14 Aug 2025 21:06:11 +1000 Subject: [PATCH 02/13] Vision refactoring --- src/main/java/frc/robot/Robot.java | 4 + src/main/java/frc/robot/Superstructure.java | 12 +- .../subsystems/{ => vision}/Limelight.java | 64 ++----- .../frc/robot/subsystems/vision/Vision.java | 158 ++++++++++++++++++ 4 files changed, 180 insertions(+), 58 deletions(-) rename src/main/java/frc/robot/subsystems/{ => vision}/Limelight.java (73%) create mode 100644 src/main/java/frc/robot/subsystems/vision/Vision.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2e47eec..81b2fd0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,12 +6,15 @@ import com.ctre.phoenix6.SignalLogger; +import edu.wpi.first.epilogue.Epilogue; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +@Logged public class Robot extends TimedRobot { private Command m_autonomousCommand; @@ -24,6 +27,7 @@ public Robot() SignalLogger.enableAutoLogging(false); DataLogManager.start("/home/lvuser/logs"); DriverStation.startDataLog(DataLogManager.getLog()); + Epilogue.bind(this); } @Override diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index dbb4377..b6fe265 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -4,6 +4,7 @@ import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -27,9 +28,9 @@ import frc.robot.constants.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.CoralRoller; -import frc.robot.subsystems.Limelight; import frc.robot.subsystems.RumbleRequester; -import frc.robot.subsystems.Limelight.TagPOI; +import frc.robot.subsystems.vision.Limelight; +import frc.robot.subsystems.vision.Limelight.TagPOI; import frc.robot.util.AutoFactories; import frc.robot.util.FieldUtils; import frc.robot.util.SD; @@ -255,11 +256,14 @@ public void periodic() { updateSwerveState(); + double yaw = getYaw(); + SD.SENSOR_GYRO.put(yaw); + if (SD.IO_LL.get()) { for (Limelight ll : limelights) { - ll.fetchVisionValues().ifPresent + ll.fetchVisionValues(yaw).ifPresent ( visionVals -> { @@ -302,7 +306,7 @@ private void setStartPose(boolean isRedAlliance) public static boolean isVisionActive() {return useLimelights;} public static boolean isRestrictorsActive() {return useRestrictors;} public static void setYaw(double newYaw) {s_Swerve.getPigeon2().setYaw(newYaw);} - public static double getYaw() {return s_Swerve.getPigeon2().getYaw().getValueAsDouble();} + public double getYaw() {return s_Swerve.getPigeon2().getYaw().getValueAsDouble();} public static boolean checkTargetPosition(TargetPosition testTargetPosition) {return testTargetPosition == currentTarget;} public static boolean checkDriveState(DriveState testDriveState) {return testDriveState == currentDriveState;} } diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/vision/Limelight.java similarity index 73% rename from src/main/java/frc/robot/subsystems/Limelight.java rename to src/main/java/frc/robot/subsystems/vision/Limelight.java index 25cc4f6..fa085a6 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/vision/Limelight.java @@ -2,13 +2,11 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.subsystems; +package frc.robot.subsystems.vision; import java.util.ArrayList; import java.util.Optional; -import com.ctre.phoenix6.Utils; - import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; @@ -26,14 +24,12 @@ import frc.robot.util.LimelightHelpers.PoseEstimate; import frc.robot.util.LimelightHelpers; -public class Limelight extends SubsystemBase +public class Limelight { private boolean useUpdate; private LimelightHelpers.PoseEstimate mt2; - private static int[] validIDs = Constants.Vision.reefIDs; private LimelightHelpers.PoseEstimate mt1; - private double headingDeg; private double omegaRps; private double stdDevFactor; private double linearStdDev; @@ -41,7 +37,6 @@ public class Limelight extends SubsystemBase private final String limelightName; - private int pipelineIndex = (int)SD.IO_LL_EXPOSURE.defaultValue(); public static boolean rotationKnown = false; private ArrayList rotationData = new ArrayList(); private boolean lastCycleRotationKnown = false; @@ -59,7 +54,6 @@ public enum TagPOI public Limelight(String name) { limelightName = name; - LimelightHelpers.setPipelineIndex(limelightName, pipelineIndex); } public void setIMUMode(int mode) @@ -74,49 +68,25 @@ public Rotation2d getLimelightRotation() return Rotation2d.kZero; } - public void setThrottle(int throttle) + protected Limelight updateValidIDs(int[] validIDs) { - NetworkTableInstance.getDefault().getTable(limelightName).getEntry("").setNumber(throttle); + LimelightHelpers.SetFiducialIDFiltersOverride(limelightName, validIDs); + return this; } - public static void setActivePOI(TagPOI activePOI) + protected Limelight updatePipeline(int pipelineIndex) { - switch (activePOI) - { - default: - case REEF: - validIDs = Constants.Vision.reefIDs; - break; - case BARGE: - validIDs = Constants.Vision.bargeIDs; - break; - case PROCESSOR: - case CORALSTATION: - validIDs = Constants.Vision.humanPlayerStationIDs; - break; - } - } - - public int updateLimelightPipeline() - { - if (SD.IO_LL_EXPOSURE_UP.button()) - { - SD.IO_LL_EXPOSURE.put(MathUtil.clamp(SD.IO_LL_EXPOSURE.get().intValue() + 1, 0, 7)); - } - if (SD.IO_LL_EXPOSURE_DOWN.button()) - { - SD.IO_LL_EXPOSURE.put(MathUtil.clamp(SD.IO_LL_EXPOSURE.get().intValue() - 1, 0, 7)); - } - - return SD.IO_LL_EXPOSURE.get().intValue(); + LimelightHelpers.setPipelineIndex(limelightName, pipelineIndex); + return this; } @Logged public Pose3d getMT1Pose() {return LimelightHelpers.getBotPose3d_wpiBlue(limelightName);} - public Optional, PoseEstimate>> fetchVisionValues() + public Optional, PoseEstimate>> fetchVisionValues(double headingDeg) { + LimelightHelpers.SetRobotOrientation(limelightName, headingDeg, 0, 0, 0, 0, 0); mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName); //mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName); @@ -134,7 +104,6 @@ public Optional, PoseEstimate>> fetchVisionValues() return Optional.empty(); } - @Override public void periodic() { if (Superstructure.isVisionActive()) @@ -183,20 +152,7 @@ public void periodic() } } - if (updateLimelightPipeline() != pipelineIndex) - { - pipelineIndex = updateLimelightPipeline(); - LimelightHelpers.setPipelineIndex(limelightName, pipelineIndex); - } - - headingDeg = Superstructure.getYaw(); //omegaRps = Units.radiansToRotations(RobotContainer.swerveState.Speeds.omegaRadiansPerSecond); - - LimelightHelpers.SetRobotOrientation(limelightName, headingDeg, 0, 0, 0, 0, 0); - - LimelightHelpers.SetFiducialIDFiltersOverride(limelightName, validIDs); - - SD.SENSOR_GYRO.put(headingDeg); } } } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java new file mode 100644 index 0000000..258d8a3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -0,0 +1,158 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.vision; + +import java.util.function.Supplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Superstructure; +import frc.robot.constants.Constants; +import frc.robot.subsystems.vision.Limelight.TagPOI; +import frc.robot.util.LimelightHelpers; +import frc.robot.util.SD; + +public class Vision extends SubsystemBase +{ + private final PoseEstimateConsumer consumer; + private final Supplier gyro; + private final Limelight[] lls; + + private final TimeInterpolatableBuffer headingBuffer = + TimeInterpolatableBuffer.createBuffer(1.0); + private static int[] validIDs = Constants.Vision.reefIDs; + + private int pipelineIndex = (int)SD.IO_LL_EXPOSURE.defaultValue(); + private boolean pipelineUpdated = false; + + /** Creates a new Vision. */ + public Vision(PoseEstimateConsumer consumer, Supplier gyro, Limelight... lls) + { + this.consumer = consumer; + this.gyro = gyro; + this.lls = lls; + } + + public void setActivePOI(TagPOI activePOI) + { + switch (activePOI) + { + default: + case REEF: + validIDs = Constants.Vision.reefIDs; + break; + case BARGE: + validIDs = Constants.Vision.bargeIDs; + break; + case PROCESSOR: + case CORALSTATION: + validIDs = Constants.Vision.humanPlayerStationIDs; + break; + } + } + + public int updatePipeline() + { + if (SD.IO_LL_EXPOSURE_UP.button()) + { + SD.IO_LL_EXPOSURE.put(MathUtil.clamp(SD.IO_LL_EXPOSURE.get().intValue() + 1, 0, 7)); + } + if (SD.IO_LL_EXPOSURE_DOWN.button()) + { + SD.IO_LL_EXPOSURE.put(MathUtil.clamp(SD.IO_LL_EXPOSURE.get().intValue() - 1, 0, 7)); + } + + return SD.IO_LL_EXPOSURE.get().intValue(); + } + + @Override + public void periodic() + { + headingBuffer.addSample(RobotController.getTime(), gyro.get()); + + int currentPipeline = updatePipeline(); + if (currentPipeline != pipelineIndex) + { + pipelineIndex = currentPipeline; + pipelineUpdated = true; + } + + for (var ll : lls) + { + ll.updateValidIDs(validIDs); + + if (pipelineUpdated) + { + ll.updatePipeline(pipelineIndex); + pipelineUpdated = false; + } + } + + if (Superstructure.isVisionActive()) + { + rotationKnown = Superstructure.isRotationKnown(); + + if (!rotationKnown) + { + lastCycleRotationKnown = false; + if (!getLimelightRotation().equals(Rotation2d.kZero)) + { + rotationData.add(0, getLimelightRotation().getDegrees()); + + if (rotationData.size() > mt1CyclesNeeded) + {rotationData.remove(mt1CyclesNeeded);} + + if (rotationData.size() == mt1CyclesNeeded) + { + double lowest = rotationData.get(0).doubleValue(); + double highest = rotationData.get(0).doubleValue(); + + for(int i = 1; i < mt1CyclesNeeded; i++) + { + lowest = Math.min(lowest, rotationData.get(i).doubleValue()); + highest = Math.max(highest, rotationData.get(i).doubleValue()); + } + + if (highest - lowest < 1) + { + rotationKnown = true; + Superstructure.setRotationKnown(true); + //SmartDashboard.putNumber("limelight " + limelightName + " average rotation reading", (highest + lowest) / 2); + Superstructure.setYaw((highest + lowest) / 2); + } + } + } + } + + if (!lastCycleRotationKnown) + { + if (rotationKnown) + { + rotationData.clear(); + lastCycleRotationKnown = true; + //RobotContainer.s_Swerve.resetPose(new Pose2d(RobotContainer.swerveState.Pose.getTranslation(), new Rotation2d(Math.toRadians(RobotContainer.s_Swerve.getPigeon2().getYaw().getValueAsDouble())))); + } + } + } + } + + @FunctionalInterface + public static interface PoseEstimateConsumer + { + public void accept + ( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs + ); + } +} From ea5cee9ddb76c3d94352c6e8890ffa265a199548 Mon Sep 17 00:00:00 2001 From: ThePixelatedCat Date: Sat, 16 Aug 2025 19:11:09 +1000 Subject: [PATCH 03/13] Moving static values in controlTransmut stuff into constants, continued refactor of vision systems. General encapsulation improvements. --- src/main/java/frc/robot/Superstructure.java | 61 ++++----- .../java/frc/robot/constants/Constants.java | 2 - .../frc/robot/constants/FieldConstants.java | 21 ++++ .../robot/subsystems/vision/Limelight.java | 41 +------ .../frc/robot/subsystems/vision/Vision.java | 116 ++++++++++-------- src/main/java/frc/robot/util/SD.java | 2 +- .../util/controlTransmutation/Attractor.java | 12 +- .../controlTransmutation/FieldObject.java | 11 -- .../util/controlTransmutation/Restrictor.java | 3 +- .../controlTransmutation/geoFence/Box.java | 1 + .../controlTransmutation/geoFence/Fence.java | 1 + .../controlTransmutation/geoFence/Line.java | 1 + .../controlTransmutation/geoFence/Point.java | 1 + .../geoFence/Polygon.java | 1 + .../controlTransmutation/restrictor/Box.java | 1 + .../restrictor/Polygon.java | 1 + 16 files changed, 130 insertions(+), 146 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index b6fe265..811aa18 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -1,11 +1,10 @@ package frc.robot; -import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; -import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -24,6 +23,7 @@ import frc.robot.commands.swerve.TargetScoreDrive; import frc.robot.commands.swerve.TargetStationDrive; import frc.robot.constants.Constants; +import frc.robot.subsystems.vision.Vision; import frc.robot.constants.FieldConstants; import frc.robot.constants.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -51,17 +51,16 @@ public enum DriveState {Reef, Station, Barge, None} private static boolean useFence = true; private static boolean useRestrictors = true; private boolean redAlliance; - - private final Telemetry logger; + private SwerveDriveState swerveState; private Field2d field; - private static CommandSwerveDrivetrain s_Swerve; - private CoralRoller s_Coral; - private Limelight s_foreLL; - private Limelight s_aftLL; - private Limelight[] limelights = {s_foreLL, s_aftLL}; + private final Telemetry logger; + + private final CommandSwerveDrivetrain s_Swerve; + private final CoralRoller s_Coral; + private final Vision s_Vision; private static TargetPosition currentTarget; private static DriveState currentDriveState; @@ -83,8 +82,7 @@ public Superstructure() field = new Field2d(); s_Swerve = TunerConstants.createDrivetrain(); s_Coral = new CoralRoller(); - s_foreLL = new Limelight("limelight-fore"); - s_aftLL = new Limelight("limelight-aft"); + s_Vision = new Vision(this::applyVisionEstimate, () -> Pair.of(getYaw(), swerveState.Speeds.omegaRadiansPerSecond), new Limelight("limelight-fore"), new Limelight("limelight-aft")); logger = new Telemetry(Constants.Swerve.maxSpeed); @@ -105,6 +103,7 @@ public Superstructure() bindControls(); bindRumbles(); + bindSD(); } public SwerveDriveState getSwerveState() @@ -136,7 +135,7 @@ private void bindControls() ); new Trigger(() -> {return currentDriveState == DriveState.None;}) - .onTrue(Commands.runOnce(() -> Limelight.setActivePOI(TagPOI.REEF))) + .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.REEF))) .whileTrue ( new ManualDrive @@ -149,7 +148,7 @@ private void bindControls() ); new Trigger(() -> {return currentDriveState == DriveState.Reef;}) - .onTrue(Commands.runOnce(() -> Limelight.setActivePOI(TagPOI.REEF))) + .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.REEF))) .whileTrue ( new TargetScoreDrive @@ -162,7 +161,7 @@ private void bindControls() ); new Trigger(() -> {return currentDriveState == DriveState.Station;}) - .onTrue(Commands.runOnce(() -> Limelight.setActivePOI(TagPOI.CORALSTATION))) + .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.CORALSTATION))) .whileTrue ( new TargetStationDrive @@ -175,7 +174,7 @@ private void bindControls() ); new Trigger(() -> {return currentDriveState == DriveState.Barge;}) - .onTrue(Commands.runOnce(() -> Limelight.setActivePOI(TagPOI.BARGE))) + .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.BARGE))) .whileTrue ( new HeadingLockedDrive @@ -252,30 +251,24 @@ private void bindRumbles() io_copilotRight.addRumbleTrigger("ready to score" , new Trigger(() -> FieldUtils.atReefLineUp(swerveState.Pose.getTranslation()))); } + private void bindSD() + { + new Trigger(SD.IO_LL_EXPOSURE_UP::button).onTrue(Commands.runOnce(s_Vision::incrementPipeline)); + new Trigger(SD.IO_LL_EXPOSURE_DOWN::button).onTrue(Commands.runOnce(s_Vision::decrementPipeline)); + } + + private void applyVisionEstimate(Pose2d poseEstMeters, double timestampSeconds, Matrix stdDevs) + { + s_Swerve.setVisionMeasurementStdDevs(stdDevs); + s_Swerve.addVisionMeasurement(poseEstMeters, timestampSeconds); + } + public void periodic() { updateSwerveState(); double yaw = getYaw(); SD.SENSOR_GYRO.put(yaw); - - if (SD.IO_LL.get()) - { - for (Limelight ll : limelights) - { - ll.fetchVisionValues(yaw).ifPresent - ( - visionVals -> - { - var stdDevs = visionVals.getFirst(); - var mt2 = visionVals.getSecond(); - - s_Swerve.setVisionMeasurementStdDevs(stdDevs); - s_Swerve.addVisionMeasurement(mt2.pose, Utils.fpgaToCurrentTime(mt2.timestampSeconds)); - } - ); - } - } } public Command getAutonomousCommand() @@ -306,7 +299,7 @@ private void setStartPose(boolean isRedAlliance) public static boolean isVisionActive() {return useLimelights;} public static boolean isRestrictorsActive() {return useRestrictors;} public static void setYaw(double newYaw) {s_Swerve.getPigeon2().setYaw(newYaw);} - public double getYaw() {return s_Swerve.getPigeon2().getYaw().getValueAsDouble();} + public double getYaw() {return s_Swerve.getPigeon2().getYaw().getValueAsDouble();} public static boolean checkTargetPosition(TargetPosition testTargetPosition) {return testTargetPosition == currentTarget;} public static boolean checkDriveState(DriveState testDriveState) {return testDriveState == currentDriveState;} } diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 6d911bb..2185d2c 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -36,8 +36,6 @@ public static final class Control public static final double lineupTolerance = 0.05; } - - public static final class Swerve { /** Centre-centre distance (length and width) between wheels, metres */ diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java index 0332554..580bed0 100644 --- a/src/main/java/frc/robot/constants/FieldConstants.java +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -99,6 +99,17 @@ public static Pose2d getLineup(String name) public static final class GeoFencing { + /** + * Minimum value for object radius, metres

+ * The system is not confirmed to handle negative radii + */ + public static final double minRadius = 0; + /** + * Minimum value for object buffer, metres

+ * A small buffer is required to ensure safe transitions + */ + public static final double minBuffer = 0.1; + // Relative to the centre of the robot, in direction the robot is facing // These values are the distance in metres to the virtual wall the robot will stop at // 0 means the wall is running through the middle of the robot @@ -218,5 +229,15 @@ public static final class GeoFencing public static final ObjectList fieldGeoFence = new ObjectList(field, fieldBlueGeoFence, fieldRedGeoFence); + /** Minimum speed limit within a restrictor */ + public static final double minLocalSpeedLimit = 0.05; + } + + public static final class AutoDrive + { + /** Attractor minimum angle tolerance, degrees */ + public static final double minAngleTolerance = 20; + /** Attractor maximum angle tolerance, degrees */ + public static final double maxAngleTolerance = 60; } } diff --git a/src/main/java/frc/robot/subsystems/vision/Limelight.java b/src/main/java/frc/robot/subsystems/vision/Limelight.java index fa085a6..790e4c5 100644 --- a/src/main/java/frc/robot/subsystems/vision/Limelight.java +++ b/src/main/java/frc/robot/subsystems/vision/Limelight.java @@ -5,36 +5,19 @@ package frc.robot.subsystems.vision; import java.util.ArrayList; -import java.util.Optional; import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Superstructure; -import frc.robot.constants.Constants; -import frc.robot.util.SD; import frc.robot.util.LimelightHelpers.PoseEstimate; import frc.robot.util.LimelightHelpers; public class Limelight { - private boolean useUpdate; private LimelightHelpers.PoseEstimate mt2; private LimelightHelpers.PoseEstimate mt1; - private double omegaRps; - private double stdDevFactor; - private double linearStdDev; - private double rotStdDev; - private final String limelightName; public static boolean rotationKnown = false; @@ -68,40 +51,24 @@ public Rotation2d getLimelightRotation() return Rotation2d.kZero; } - protected Limelight updateValidIDs(int[] validIDs) + protected void updateValidIDs(int[] validIDs) { LimelightHelpers.SetFiducialIDFiltersOverride(limelightName, validIDs); - return this; } - protected Limelight updatePipeline(int pipelineIndex) + protected void updatePipeline(int pipelineIndex) { LimelightHelpers.setPipelineIndex(limelightName, pipelineIndex); - return this; } @Logged public Pose3d getMT1Pose() {return LimelightHelpers.getBotPose3d_wpiBlue(limelightName);} - public Optional, PoseEstimate>> fetchVisionValues(double headingDeg) + public PoseEstimate getMT2(double headingDeg) { LimelightHelpers.SetRobotOrientation(limelightName, headingDeg, 0, 0, 0, 0, 0); - mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName); - //mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName); - - useUpdate = !(mt2 == null || mt2.tagCount == 0 || omegaRps > 2.0); - - if (useUpdate) - { - stdDevFactor = Math.pow(mt2.avgTagDist, 2.0) / mt2.tagCount; - - linearStdDev = Constants.Vision.linearStdDevBaseline * stdDevFactor; - rotStdDev = Constants.Vision.rotStdDevBaseline * stdDevFactor; - - return Optional.of(Pair.of(VecBuilder.fill(linearStdDev, linearStdDev, rotStdDev), mt2)); - } - return Optional.empty(); + return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName); } public void periodic() diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 258d8a3..1fdcb0b 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -4,10 +4,16 @@ package frc.robot.subsystems.vision; +import java.util.ArrayList; +import java.util.Optional; import java.util.function.Supplier; +import com.ctre.phoenix6.Utils; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; @@ -24,102 +30,106 @@ public class Vision extends SubsystemBase { private final PoseEstimateConsumer consumer; - private final Supplier gyro; + private final Supplier> rotationData; private final Limelight[] lls; - private final TimeInterpolatableBuffer headingBuffer = - TimeInterpolatableBuffer.createBuffer(1.0); - private static int[] validIDs = Constants.Vision.reefIDs; - + private int[] validIDs = Constants.Vision.reefIDs; private int pipelineIndex = (int)SD.IO_LL_EXPOSURE.defaultValue(); - private boolean pipelineUpdated = false; + + private ArrayList rotationBuf = new ArrayList(); + private boolean lastCycleRotationKnown = false; + private final int mt1CyclesNeeded = 10; /** Creates a new Vision. */ - public Vision(PoseEstimateConsumer consumer, Supplier gyro, Limelight... lls) + public Vision(PoseEstimateConsumer consumer, Supplier> rotationData, Limelight... lls) { this.consumer = consumer; - this.gyro = gyro; + this.rotationData = rotationData; this.lls = lls; } public void setActivePOI(TagPOI activePOI) { + validIDs = switch (activePOI) { - default: - case REEF: - validIDs = Constants.Vision.reefIDs; - break; - case BARGE: - validIDs = Constants.Vision.bargeIDs; - break; - case PROCESSOR: - case CORALSTATION: - validIDs = Constants.Vision.humanPlayerStationIDs; - break; - } + case REEF -> Constants.Vision.reefIDs; + case BARGE -> Constants.Vision.bargeIDs; + case CORALSTATION, PROCESSOR -> Constants.Vision.humanPlayerStationIDs; + default -> Constants.Vision.reefIDs; + }; } - public int updatePipeline() + public void incrementPipeline() { - if (SD.IO_LL_EXPOSURE_UP.button()) - { - SD.IO_LL_EXPOSURE.put(MathUtil.clamp(SD.IO_LL_EXPOSURE.get().intValue() + 1, 0, 7)); - } - if (SD.IO_LL_EXPOSURE_DOWN.button()) - { - SD.IO_LL_EXPOSURE.put(MathUtil.clamp(SD.IO_LL_EXPOSURE.get().intValue() - 1, 0, 7)); - } + pipelineIndex = MathUtil.clamp(pipelineIndex + 1, 0, 7); + for (var ll : lls) {ll.updatePipeline(pipelineIndex);} + SD.IO_LL_EXPOSURE.put(pipelineIndex); + } - return SD.IO_LL_EXPOSURE.get().intValue(); + public void decrementPipeline() + { + pipelineIndex = MathUtil.clamp(pipelineIndex - 1, 0, 7); + for (var ll : lls) {ll.updatePipeline(pipelineIndex);} + SD.IO_LL_EXPOSURE.put(pipelineIndex); } @Override public void periodic() { - headingBuffer.addSample(RobotController.getTime(), gyro.get()); - - int currentPipeline = updatePipeline(); - if (currentPipeline != pipelineIndex) - { - pipelineIndex = currentPipeline; - pipelineUpdated = true; - } - for (var ll : lls) { ll.updateValidIDs(validIDs); + } - if (pipelineUpdated) + if (SD.IO_LL.get()) + { + for (var ll : lls) { - ll.updatePipeline(pipelineIndex); - pipelineUpdated = false; + var rotationVals = rotationData.get(); + double heading = rotationVals.getFirst(); + double omegaRps = rotationVals.getSecond(); + + var mt2 = ll.getMT2(heading); + //mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName); + + boolean useUpdate = !(mt2 == null || mt2.tagCount == 0 || omegaRps > 2.0); + + if (useUpdate) + { + double stdDevFactor = Math.pow(mt2.avgTagDist, 2.0) / mt2.tagCount; + + double linearStdDev = Constants.Vision.linearStdDevBaseline * stdDevFactor; + double rotStdDev = Constants.Vision.rotStdDevBaseline * stdDevFactor; + + consumer.accept(mt2.pose, Utils.fpgaToCurrentTime(mt2.timestampSeconds), VecBuilder.fill(linearStdDev, linearStdDev, rotStdDev)); + } } - } + } if (Superstructure.isVisionActive()) { - rotationKnown = Superstructure.isRotationKnown(); + boolean rotationKnown = Superstructure.isRotationKnown(); if (!rotationKnown) { lastCycleRotationKnown = false; if (!getLimelightRotation().equals(Rotation2d.kZero)) { - rotationData.add(0, getLimelightRotation().getDegrees()); + rotationBuf.add(0, getLimelightRotation().getDegrees()); - if (rotationData.size() > mt1CyclesNeeded) - {rotationData.remove(mt1CyclesNeeded);} + if (rotationBuf.size() > mt1CyclesNeeded) + {rotationBuf.remove(mt1CyclesNeeded);} - if (rotationData.size() == mt1CyclesNeeded) + if (rotationBuf.size() == mt1CyclesNeeded) { - double lowest = rotationData.get(0).doubleValue(); - double highest = rotationData.get(0).doubleValue(); + double lowest = rotationBuf.get(0).doubleValue(); + double highest = rotationBuf.get(0).doubleValue(); for(int i = 1; i < mt1CyclesNeeded; i++) { - lowest = Math.min(lowest, rotationData.get(i).doubleValue()); - highest = Math.max(highest, rotationData.get(i).doubleValue()); + lowest = Math.min(lowest, rotationBuf.get(i).doubleValue()); + highest = Math.max(highest, rotationBuf.get(i).doubleValue()); } if (highest - lowest < 1) @@ -137,7 +147,7 @@ public void periodic() { if (rotationKnown) { - rotationData.clear(); + rotationBuf.clear(); lastCycleRotationKnown = true; //RobotContainer.s_Swerve.resetPose(new Pose2d(RobotContainer.swerveState.Pose.getTranslation(), new Rotation2d(Math.toRadians(RobotContainer.s_Swerve.getPigeon2().getYaw().getValueAsDouble())))); } diff --git a/src/main/java/frc/robot/util/SD.java b/src/main/java/frc/robot/util/SD.java index 27a4089..e103470 100644 --- a/src/main/java/frc/robot/util/SD.java +++ b/src/main/java/frc/robot/util/SD.java @@ -113,7 +113,7 @@ public record BooleanKey (String label, boolean defaultValue) implements Initabl public boolean button() { - if (SmartDashboard.getBoolean(label, defaultValue)) + if (get()) { put(false); return true; diff --git a/src/main/java/frc/robot/util/controlTransmutation/Attractor.java b/src/main/java/frc/robot/util/controlTransmutation/Attractor.java index 0134c7d..8bbcd71 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/Attractor.java +++ b/src/main/java/frc/robot/util/controlTransmutation/Attractor.java @@ -4,6 +4,8 @@ import edu.wpi.first.math.geometry.Translation2d; import frc.robot.util.Conversions; +import static frc.robot.constants.FieldConstants.AutoDrive.*; + /** Guides the robot towards a point along a given heading */ public class Attractor extends FieldObject { @@ -15,10 +17,6 @@ public class Attractor extends FieldObject protected Translation2d frontCheckpoint; /** Point opposite where the approach heading intersects the effect radius */ protected Translation2d backCheckpoint; - /** Minimum angle tollerance, degrees */ - protected static final double minAngleTollerance = 20; - /** Maximum angle tollerance, degrees */ - protected static final double maxAngleTollerance = 60; /** Scalar for how far to back off from the target when approaching from the side */ protected double approachScalar = 0.1; /** Input scale for approaching within the buffer based on distance */ @@ -94,7 +92,7 @@ public boolean checkAngle(Translation2d controlInput) if ( !lastInputAngle.equals(Rotation2d.kZero) && - Conversions.isRotationNear(lastInputAngle, controlInput.getAngle(), minAngleTollerance) + Conversions.isRotationNear(lastInputAngle, controlInput.getAngle(), minAngleTolerance) ) { return true; @@ -102,12 +100,12 @@ public boolean checkAngle(Translation2d controlInput) if (distance <= buffer) { - return Conversions.isRotationNear(approachHeadingRotation, controlInput.getAngle(), maxAngleTollerance); + return Conversions.isRotationNear(approachHeadingRotation, controlInput.getAngle(), maxAngleTolerance); } Rotation2d angleToTarget = centre.minus(robotPos).getAngle(); - double angleTolerance = Conversions.clamp(2*Math.atan(buffer/distance), minAngleTollerance, maxAngleTollerance); + double angleTolerance = Conversions.clamp(2*Math.atan(buffer/distance), minAngleTolerance, maxAngleTolerance); return Conversions.isRotationNear(angleToTarget, controlInput.getAngle(), angleTolerance); } diff --git a/src/main/java/frc/robot/util/controlTransmutation/FieldObject.java b/src/main/java/frc/robot/util/controlTransmutation/FieldObject.java index aed6eab..71425d6 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/FieldObject.java +++ b/src/main/java/frc/robot/util/controlTransmutation/FieldObject.java @@ -30,17 +30,6 @@ public abstract class FieldObject extends InputTransmuter /** Condition for the object to be active, if the return is false the object will return the input */ protected BooleanSupplier activeSupplier = () -> true; - /** - * Global minimum value for object radius, metres

- * The system is not confirmed to handle negative radii - */ - protected static final double minRadius = 0; - /** - * Global minimum value for object buffer, metres

- * A small buffer is required to ensure safe transitions - */ - protected static final double minBuffer = 0.1; - /** * Sets the global robot position supplier for all field objects * @param robotPosSupplier Translation2d Supplier for the robot position (not pose) diff --git a/src/main/java/frc/robot/util/controlTransmutation/Restrictor.java b/src/main/java/frc/robot/util/controlTransmutation/Restrictor.java index 1fbbe77..b7a554c 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/Restrictor.java +++ b/src/main/java/frc/robot/util/controlTransmutation/Restrictor.java @@ -4,6 +4,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import static frc.robot.constants.FieldConstants.GeoFencing.*; + /** * Derived from GeoFence logic, acts as a non-directional speed-limit within the given area

* Using a local speed limit <= 0 allows it to be used as a position/distance check @@ -11,7 +13,6 @@ public class Restrictor extends FieldObject { protected double localSpeedLimit; - protected static final double minLocalSpeedLimit = 0.05; /** * Basic point-type restrictor object diff --git a/src/main/java/frc/robot/util/controlTransmutation/geoFence/Box.java b/src/main/java/frc/robot/util/controlTransmutation/geoFence/Box.java index 3525dd6..9b911dd 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/geoFence/Box.java +++ b/src/main/java/frc/robot/util/controlTransmutation/geoFence/Box.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.geometry.Translation2d; import frc.robot.util.Conversions; import frc.robot.util.controlTransmutation.GeoFence; +import static frc.robot.constants.FieldConstants.GeoFencing.*; /** * Box type GeoFence object

diff --git a/src/main/java/frc/robot/util/controlTransmutation/geoFence/Fence.java b/src/main/java/frc/robot/util/controlTransmutation/geoFence/Fence.java index 5967237..8630703 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/geoFence/Fence.java +++ b/src/main/java/frc/robot/util/controlTransmutation/geoFence/Fence.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.geometry.Translation2d; import frc.robot.util.Conversions; import frc.robot.util.controlTransmutation.GeoFence; +import static frc.robot.constants.FieldConstants.GeoFencing.*; /** * Fence type GeoFence object

diff --git a/src/main/java/frc/robot/util/controlTransmutation/geoFence/Line.java b/src/main/java/frc/robot/util/controlTransmutation/geoFence/Line.java index 85862ad..1257680 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/geoFence/Line.java +++ b/src/main/java/frc/robot/util/controlTransmutation/geoFence/Line.java @@ -11,6 +11,7 @@ import frc.robot.util.Conversions; import frc.robot.util.controlTransmutation.Attractor; import frc.robot.util.controlTransmutation.GeoFence; +import static frc.robot.constants.FieldConstants.GeoFencing.*; /** * Line type GeoFence object

diff --git a/src/main/java/frc/robot/util/controlTransmutation/geoFence/Point.java b/src/main/java/frc/robot/util/controlTransmutation/geoFence/Point.java index 86519fb..82a6758 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/geoFence/Point.java +++ b/src/main/java/frc/robot/util/controlTransmutation/geoFence/Point.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Translation2d; import frc.robot.util.controlTransmutation.GeoFence; +import static frc.robot.constants.FieldConstants.GeoFencing.*; /** * Point type GeoFence object

diff --git a/src/main/java/frc/robot/util/controlTransmutation/geoFence/Polygon.java b/src/main/java/frc/robot/util/controlTransmutation/geoFence/Polygon.java index 4baecc9..885471e 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/geoFence/Polygon.java +++ b/src/main/java/frc/robot/util/controlTransmutation/geoFence/Polygon.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.geometry.Translation2d; import frc.robot.util.Conversions; import frc.robot.util.controlTransmutation.GeoFence; +import static frc.robot.constants.FieldConstants.GeoFencing.*; /** * Polygon type GeoFence object

diff --git a/src/main/java/frc/robot/util/controlTransmutation/restrictor/Box.java b/src/main/java/frc/robot/util/controlTransmutation/restrictor/Box.java index 673905f..40f0bc4 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/restrictor/Box.java +++ b/src/main/java/frc/robot/util/controlTransmutation/restrictor/Box.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Translation2d; import frc.robot.util.controlTransmutation.Restrictor; +import static frc.robot.constants.FieldConstants.GeoFencing.*; /** Add your docs here. */ public class Box extends Restrictor diff --git a/src/main/java/frc/robot/util/controlTransmutation/restrictor/Polygon.java b/src/main/java/frc/robot/util/controlTransmutation/restrictor/Polygon.java index 9cbff9a..9fb451a 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/restrictor/Polygon.java +++ b/src/main/java/frc/robot/util/controlTransmutation/restrictor/Polygon.java @@ -10,6 +10,7 @@ import edu.wpi.first.math.geometry.Translation2d; import frc.robot.util.Conversions; import frc.robot.util.controlTransmutation.Restrictor; +import static frc.robot.constants.FieldConstants.GeoFencing.*; /** Add your docs here. */ public class Polygon extends Restrictor From fe0e648dafb599416a163d4a6562c97eb38d25d6 Mon Sep 17 00:00:00 2001 From: ThePixelatedCat Date: Mon, 18 Aug 2025 20:47:29 +1000 Subject: [PATCH 04/13] Adjusted attractor setup, cleaned up superstructure imports and uneeded stuff, used reflection to auto-init everything in SD, removed unneeded keys, simplified coralroller, removed algaeroller, simplified RumbleRequester, renamed copilot to operator, organised utils a bit, changed switches to new syntax --- src/main/java/frc/robot/Robot.java | 20 ++-- src/main/java/frc/robot/Superstructure.java | 86 +++++++--------- .../commands/swerve/TargetScoreDrive.java | 51 +++++----- .../java/frc/robot/constants/Constants.java | 2 +- .../frc/robot/constants/FieldConstants.java | 98 +++++++------------ .../frc/robot/subsystems/AlgaeRoller.java | 47 --------- .../frc/robot/subsystems/CoralRoller.java | 26 +---- .../frc/robot/subsystems/RumbleRequester.java | 49 ++++------ .../robot/subsystems/vision/Limelight.java | 6 +- .../frc/robot/subsystems/vision/Vision.java | 10 +- .../java/frc/robot/util/AutoFactories.java | 17 +--- src/main/java/frc/robot/util/SD.java | 88 ++++++----------- .../util/{ => libs}/LimelightHelpers.java | 2 +- .../frc/robot/util/{ => libs}/Telemetry.java | 2 +- 14 files changed, 169 insertions(+), 335 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/AlgaeRoller.java rename src/main/java/frc/robot/util/{ => libs}/LimelightHelpers.java (99%) rename src/main/java/frc/robot/util/{ => libs}/Telemetry.java (99%) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 81b2fd0..5ac79ae 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,7 +15,8 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; @Logged -public class Robot extends TimedRobot { +public class Robot extends TimedRobot +{ private Command m_autonomousCommand; private final Superstructure superstructure; @@ -47,12 +48,11 @@ public void disabledPeriodic() {} public void disabledExit() {} @Override - public void autonomousInit() { + public void autonomousInit() + { m_autonomousCommand = superstructure.getAutonomousCommand(); - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - } + if (m_autonomousCommand != null) m_autonomousCommand.schedule(); } @Override @@ -62,10 +62,9 @@ public void autonomousPeriodic() {} public void autonomousExit() {} @Override - public void teleopInit() { - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); - } + public void teleopInit() + { + if (m_autonomousCommand != null) m_autonomousCommand.cancel(); } @Override @@ -75,7 +74,8 @@ public void teleopPeriodic() {} public void teleopExit() {} @Override - public void testInit() { + public void testInit() + { CommandScheduler.getInstance().cancelAll(); } diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 811aa18..9f4a52f 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -1,8 +1,5 @@ package frc.robot; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; - import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; @@ -18,28 +15,20 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.commands.swerve.HeadingLockedDrive; -import frc.robot.commands.swerve.ManualDrive; -import frc.robot.commands.swerve.TargetScoreDrive; -import frc.robot.commands.swerve.TargetStationDrive; -import frc.robot.constants.Constants; -import frc.robot.subsystems.vision.Vision; -import frc.robot.constants.FieldConstants; -import frc.robot.constants.TunerConstants; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.CoralRoller; -import frc.robot.subsystems.RumbleRequester; -import frc.robot.subsystems.vision.Limelight; + +import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; + +import frc.robot.commands.swerve.*; +import frc.robot.constants.*; +import frc.robot.subsystems.*; +import frc.robot.subsystems.vision.*; import frc.robot.subsystems.vision.Limelight.TagPOI; import frc.robot.util.AutoFactories; import frc.robot.util.FieldUtils; import frc.robot.util.SD; -import frc.robot.util.Telemetry; -import frc.robot.util.controlTransmutation.Brake; -import frc.robot.util.controlTransmutation.Deadband; -import frc.robot.util.controlTransmutation.FieldObject; -import frc.robot.util.controlTransmutation.InputCurve; -import frc.robot.util.controlTransmutation.JoystickTransmuter; +import frc.robot.util.controlTransmutation.*; +import frc.robot.util.libs.Telemetry; public class Superstructure { @@ -49,28 +38,26 @@ public enum DriveState {Reef, Station, Barge, None} private static boolean rotationKnown = false; private static boolean useLimelights = true; private static boolean useFence = true; - private static boolean useRestrictors = true; private boolean redAlliance; - private SwerveDriveState swerveState; private Field2d field; - private final Telemetry logger; + private final Telemetry ctreLogger; private final CommandSwerveDrivetrain s_Swerve; private final CoralRoller s_Coral; private final Vision s_Vision; - private static TargetPosition currentTarget; - private static DriveState currentDriveState; + private TargetPosition currentTarget; + private DriveState currentDriveState; private final CommandXboxController driver = new CommandXboxController(0); private final CommandXboxController operator = new CommandXboxController(1); - private final RumbleRequester io_driverRight = new RumbleRequester(driver, RumbleType.kRightRumble, SD.RUMBLE_D_R::put, SD.IO_RUMBLE_D::get); - private final RumbleRequester io_driverLeft = new RumbleRequester(driver, RumbleType.kLeftRumble, SD.RUMBLE_D_L::put, SD.IO_RUMBLE_D::get); - private final RumbleRequester io_copilotRight = new RumbleRequester(operator, RumbleType.kRightRumble, SD.RUMBLE_C_R::put, SD.IO_RUMBLE_C::get); - private final RumbleRequester io_copilotLeft = new RumbleRequester(operator, RumbleType.kLeftRumble, SD.RUMBLE_C_L::put, SD.IO_RUMBLE_C::get); + private final RumbleRequester io_driverRight = new RumbleRequester(driver, RumbleType.kRightRumble, SD.RUMBLE_DRIVER::get); + private final RumbleRequester io_driverLeft = new RumbleRequester(driver, RumbleType.kLeftRumble, SD.RUMBLE_DRIVER::get); + private final RumbleRequester io_operatorRight = new RumbleRequester(operator, RumbleType.kRightRumble, SD.RUMBLE_OPERATOR::get); + private final RumbleRequester io_operatorLeft = new RumbleRequester(operator, RumbleType.kLeftRumble, SD.RUMBLE_OPERATOR::get); private final JoystickTransmuter driverStick = new JoystickTransmuter(driver::getLeftY, driver::getLeftX).invertX().invertY(); private final Brake driverBrake = new Brake(driver::getRightTriggerAxis, Constants.Control.maxThrottle, Constants.Control.minThrottle); @@ -84,7 +71,7 @@ public Superstructure() s_Coral = new CoralRoller(); s_Vision = new Vision(this::applyVisionEstimate, () -> Pair.of(getYaw(), swerveState.Speeds.omegaRadiansPerSecond), new Limelight("limelight-fore"), new Limelight("limelight-aft")); - logger = new Telemetry(Constants.Swerve.maxSpeed); + ctreLogger = new Telemetry(Constants.Swerve.maxSpeed); redAlliance = FieldUtils.isRedAlliance(); @@ -95,9 +82,10 @@ public Superstructure() SmartDashboard.putData("Field", field); - s_Swerve.registerTelemetry(logger::telemeterize); + s_Swerve.registerTelemetry(ctreLogger::telemeterize); driverStick.withFieldObjects(FieldConstants.GeoFencing.fieldGeoFence).withBrake(driverBrake).withInputCurve(driverInputCurve).withDeadband(driverDeadband); FieldConstants.GeoFencing.fieldGeoFence.setActiveCondition(() -> useFence && useLimelights); + FieldConstants.GeoFencing.configureAttractors((testTarget, testState) -> currentTarget == testTarget && currentDriveState == testState); FieldObject.setRobotRadiusSup(this::robotRadiusSup); FieldObject.setRobotPosSup(this::getPosition); @@ -134,7 +122,9 @@ private void bindControls() ) ); - new Trigger(() -> {return currentDriveState == DriveState.None;}) + s_Coral.setDefaultCommand(s_Coral.setSpeedCommand(0)); + + new Trigger(() -> currentDriveState == DriveState.None) .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.REEF))) .whileTrue ( @@ -147,7 +137,7 @@ private void bindControls() ) ); - new Trigger(() -> {return currentDriveState == DriveState.Reef;}) + new Trigger(() -> currentDriveState == DriveState.Reef) .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.REEF))) .whileTrue ( @@ -160,7 +150,7 @@ private void bindControls() ) ); - new Trigger(() -> {return currentDriveState == DriveState.Station;}) + new Trigger(() -> currentDriveState == DriveState.Station) .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.CORALSTATION))) .whileTrue ( @@ -173,7 +163,7 @@ private void bindControls() ) ); - new Trigger(() -> {return currentDriveState == DriveState.Barge;}) + new Trigger(() -> currentDriveState == DriveState.Barge) .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.BARGE))) .whileTrue ( @@ -188,8 +178,8 @@ private void bindControls() ); - driver.leftTrigger().whileTrue(s_Coral.runCommand(SD.IO_CORALSPEED_F.get())); - driver.leftBumper().whileTrue(s_Coral.runCommand(SD.IO_CORALSPEED_R.get())); + driver.leftTrigger().whileTrue(s_Coral.setSpeedCommand(Constants.Coral.forwardSpeed)); + driver.leftBumper().whileTrue(s_Coral.setSpeedCommand(Constants.Coral.reverseSpeed)); //operator.leftBumper().whileTrue(s_Coral.runCommand(SD.IO_CORALSPEED_R.get())); // Heading reset driver.start() @@ -242,19 +232,19 @@ private void bindControls() driver.b().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.None)); driver.axisMagnitudeGreaterThan(Axis.kRightX.value, 0.2).onTrue(Commands.runOnce(() -> currentDriveState = DriveState.None)); - new Trigger(() -> FieldUtils.atReefLineUp(swerveState.Pose.getTranslation())).whileTrue(s_Coral.smartRunCommand(Constants.Coral.forwardSpeed)); + new Trigger(() -> FieldUtils.atReefLineUp(swerveState.Pose.getTranslation())).whileTrue(s_Coral.setSpeedCommand(Constants.Coral.forwardSpeed)); } private void bindRumbles() { - io_copilotLeft.addRumbleTrigger("coral held", new Trigger(s_Coral::getSensor)); - io_copilotRight.addRumbleTrigger("ready to score" , new Trigger(() -> FieldUtils.atReefLineUp(swerveState.Pose.getTranslation()))); + io_operatorLeft.addRumbleTrigger("CoralHeld", new Trigger(s_Coral::getSensor)); + io_operatorRight.addRumbleTrigger("ScoreReady" , new Trigger(() -> FieldUtils.atReefLineUp(swerveState.Pose.getTranslation()))); } private void bindSD() { - new Trigger(SD.IO_LL_EXPOSURE_UP::button).onTrue(Commands.runOnce(s_Vision::incrementPipeline)); - new Trigger(SD.IO_LL_EXPOSURE_DOWN::button).onTrue(Commands.runOnce(s_Vision::decrementPipeline)); + new Trigger(SD.LL_EXPOSURE_UP::button).onTrue(Commands.runOnce(s_Vision::incrementPipeline)); + new Trigger(SD.LL_EXPOSURE_DOWN::button).onTrue(Commands.runOnce(s_Vision::decrementPipeline)); } private void applyVisionEstimate(Pose2d poseEstMeters, double timestampSeconds, Matrix stdDevs) @@ -266,14 +256,11 @@ private void applyVisionEstimate(Pose2d poseEstMeters, double timestampSeconds, public void periodic() { updateSwerveState(); - - double yaw = getYaw(); - SD.SENSOR_GYRO.put(yaw); } public Command getAutonomousCommand() { - return AutoFactories.getCommandList(SD.IO_AUTO.get(), s_Coral, s_Swerve, () -> swerveState); + return AutoFactories.getCommandList(SD.AUTO_STRING.get(), s_Coral, s_Swerve, () -> swerveState); } public double robotRadiusSup() @@ -297,9 +284,6 @@ private void setStartPose(boolean isRedAlliance) public static boolean isRotationKnown() {return rotationKnown;} public static void setRotationKnown(boolean isKnown) {rotationKnown = isKnown;} public static boolean isVisionActive() {return useLimelights;} - public static boolean isRestrictorsActive() {return useRestrictors;} - public static void setYaw(double newYaw) {s_Swerve.getPigeon2().setYaw(newYaw);} + public void setYaw(double newYaw) {s_Swerve.getPigeon2().setYaw(newYaw);} public double getYaw() {return s_Swerve.getPigeon2().getYaw().getValueAsDouble();} - public static boolean checkTargetPosition(TargetPosition testTargetPosition) {return testTargetPosition == currentTarget;} - public static boolean checkDriveState(DriveState testDriveState) {return testDriveState == currentDriveState;} } diff --git a/src/main/java/frc/robot/commands/swerve/TargetScoreDrive.java b/src/main/java/frc/robot/commands/swerve/TargetScoreDrive.java index 9b9f78c..671d29a 100644 --- a/src/main/java/frc/robot/commands/swerve/TargetScoreDrive.java +++ b/src/main/java/frc/robot/commands/swerve/TargetScoreDrive.java @@ -5,17 +5,13 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; -import frc.robot.constants.FieldConstants; +import static frc.robot.constants.FieldConstants.GeoFencing.*; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.util.FieldUtils; -import frc.robot.util.controlTransmutation.CrossDeadband; -import frc.robot.util.controlTransmutation.Deadband; public class TargetScoreDrive extends HeadingLockedDrive { private Rotation2d rotationOffsetBase; - private int nearestReefFace; - private Deadband crossDeadband = new CrossDeadband(); /** Creates a new TargetScoreDrive. */ public TargetScoreDrive @@ -33,48 +29,45 @@ public class TargetScoreDrive extends HeadingLockedDrive @Override protected void updateTargetHeading() { - if - ( - FieldConstants.GeoFencing.reefBlue.getDistance() >= FieldConstants.GeoFencing.robotRadiusCircumscribed/2 && - FieldConstants.GeoFencing.reefRed.getDistance() >= FieldConstants.GeoFencing.robotRadiusCircumscribed/2 - ) + if (reefBlue.getDistance() >= robotRadiusCircumscribed/2 && reefRed.getDistance() >= robotRadiusCircumscribed/2) { - nearestReefFace = FieldUtils.getNearestReefFace(robotXY); - - switch (nearestReefFace) + switch (FieldUtils.getNearestReefFace(robotXY)) { - case 1: + case 1 -> + { targetHeading = Rotation2d.kZero; super.rotationOffset = this.rotationOffsetBase; - break; + } - case 2: + case 2 -> + { targetHeading = new Rotation2d(Units.degreesToRadians(60)); super.rotationOffset = this.rotationOffsetBase.unaryMinus(); - break; + } - case 3: + case 3 -> + { targetHeading = new Rotation2d(Units.degreesToRadians(120)); super.rotationOffset = this.rotationOffsetBase.unaryMinus(); - break; + } - case 4: + case 4 -> + { targetHeading = Rotation2d.k180deg; super.rotationOffset = this.rotationOffsetBase.unaryMinus(); - break; + } - case 5: + case 5 -> + { targetHeading = new Rotation2d(Units.degreesToRadians(-120)); super.rotationOffset = this.rotationOffsetBase.unaryMinus(); - break; - - case 6: + } + + case 6 -> + { targetHeading = new Rotation2d(Units.degreesToRadians(-60)); super.rotationOffset = this.rotationOffsetBase.unaryMinus(); - break; - - default: - break; + } } } } diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 2185d2c..00d1f33 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -7,7 +7,7 @@ public final class Constants public static final class RumblerConstants { public static final double driverDefault = 0.1; - public static final double copilotDefault = 0.1; + public static final double operatorDefault = 0.1; } public static final class Control diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java index 580bed0..95bae21 100644 --- a/src/main/java/frc/robot/constants/FieldConstants.java +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -1,12 +1,12 @@ package frc.robot.constants; import java.util.ArrayList; +import java.util.function.BiPredicate; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; -import frc.robot.Superstructure; import frc.robot.Superstructure.DriveState; import frc.robot.Superstructure.TargetPosition; import frc.robot.util.controlTransmutation.Attractor; @@ -53,46 +53,22 @@ public class FieldConstants public static Pose2d getLineup(String name) { - switch (name) { - case "ra": - return raLineup; - - case "rb": - return rbLineup; - - case "rc": - return rcLineup; - - case "rd": - return rdLineup; - - case "re": - return reLineup; - - case "rf": - return rfLineup; - - case "rg": - return rgLineup; - - case "rh": - return rhLineup; - - case "ri": - return riLineup; - - case "rj": - return rjLineup; - - case "rk": - return rkLineup; - - case "rl": - return rlLineup; - - default: - return raLineup; - } + return switch (name) + { + case "ra" -> raLineup; + case "rb" -> rbLineup; + case "rc" -> rcLineup; + case "rd" -> rdLineup; + case "re" -> reLineup; + case "rf" -> rfLineup; + case "rg" -> rgLineup; + case "rh" -> rhLineup; + case "ri" -> riLineup; + case "rj" -> rjLineup; + case "rk" -> rkLineup; + case "rl" -> rlLineup; + default -> raLineup; + }; } public static final double coralStationRange = 0.6; @@ -181,30 +157,30 @@ public static final class GeoFencing public static final Attractor testAttractor = new Attractor(fieldCentre.getX(), fieldCentre.getY(), 0, 5, 1.5); // Set up Attractors and Conditions for GeoFence objects - static + public static void configureAttractors(BiPredicate checkTargetAndState) { - reefRed.addRelativeAttractors(0.4, -0.2, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Left) && Superstructure.checkDriveState(DriveState.Reef)); - reefRed.addRelativeAttractors(0.4, 0, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Centre) && Superstructure.checkDriveState(DriveState.Reef)); - reefRed.addRelativeAttractors(0.4, 0.2, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Right) && Superstructure.checkDriveState(DriveState.Reef)); - reefBlue.addRelativeAttractors(0.4, -0.2, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Left) && Superstructure.checkDriveState(DriveState.Reef)); - reefBlue.addRelativeAttractors(0.4, 0, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Centre) && Superstructure.checkDriveState(DriveState.Reef)); - reefBlue.addRelativeAttractors(0.4, 0.2, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Right) && Superstructure.checkDriveState(DriveState.Reef)); - - cornerSBlue.addRelativeAttractor(true, 0.4, 0.5, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Left) && Superstructure.checkDriveState(DriveState.Station)); - cornerSBlue.addRelativeAttractor(true, 0.4, 0, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Centre) && Superstructure.checkDriveState(DriveState.Station)); - cornerSBlue.addRelativeAttractor(true, 0.4, -0.5, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Right) && Superstructure.checkDriveState(DriveState.Station)); + reefRed.addRelativeAttractors(0.4, -0.2, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Left, DriveState.Reef)) ; + reefRed.addRelativeAttractors(0.4, 0, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Centre, DriveState.Reef)) ; + reefRed.addRelativeAttractors(0.4, 0.2, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Right, DriveState.Reef)) ; + reefBlue.addRelativeAttractors(0.4, -0.2, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Left, DriveState.Reef)) ; + reefBlue.addRelativeAttractors(0.4, 0, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Centre, DriveState.Reef)) ; + reefBlue.addRelativeAttractors(0.4, 0.2, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Right, DriveState.Reef)) ; + + cornerSBlue.addRelativeAttractor(true, 0.4, 0.5, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Left, DriveState.Station)) ; + cornerSBlue.addRelativeAttractor(true, 0.4, 0, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Centre, DriveState.Station)) ; + cornerSBlue.addRelativeAttractor(true, 0.4, -0.5, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Right, DriveState.Station)) ; - cornerNBlue.addRelativeAttractor(false, 0.4, 0.5, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Left) && Superstructure.checkDriveState(DriveState.Station)); - cornerNBlue.addRelativeAttractor(false, 0.4, 0, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Centre) && Superstructure.checkDriveState(DriveState.Station)); - cornerNBlue.addRelativeAttractor(false, 0.4, -0.5, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Right) && Superstructure.checkDriveState(DriveState.Station)); + cornerNBlue.addRelativeAttractor(false, 0.4, 0.5, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Left, DriveState.Station)) ; + cornerNBlue.addRelativeAttractor(false, 0.4, 0, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Centre, DriveState.Station)) ; + cornerNBlue.addRelativeAttractor(false, 0.4, -0.5, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Right, DriveState.Station)) ; - cornerSRed.addRelativeAttractor(false, 0.4, 0.5, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Left) && Superstructure.checkDriveState(DriveState.Station)); - cornerSRed.addRelativeAttractor(false, 0.4, 0, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Centre) && Superstructure.checkDriveState(DriveState.Station)); - cornerSRed.addRelativeAttractor(false, 0.4, -0.5, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Right) && Superstructure.checkDriveState(DriveState.Station)); + cornerSRed.addRelativeAttractor(false, 0.4, 0.5, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Left, DriveState.Station)) ; + cornerSRed.addRelativeAttractor(false, 0.4, 0, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Centre, DriveState.Station)) ; + cornerSRed.addRelativeAttractor(false, 0.4, -0.5, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Right, DriveState.Station)) ; - cornerNRed.addRelativeAttractor(true, 0.4, 0.5, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Left) && Superstructure.checkDriveState(DriveState.Station)); - cornerNRed.addRelativeAttractor(true, 0.4, 0, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Centre) && Superstructure.checkDriveState(DriveState.Station)); - cornerNRed.addRelativeAttractor(true, 0.4, -0.5, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Right) && Superstructure.checkDriveState(DriveState.Station)); + cornerNRed.addRelativeAttractor(true, 0.4, 0.5, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Left, DriveState.Station)) ; + cornerNRed.addRelativeAttractor(true, 0.4, 0, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Centre, DriveState.Station)) ; + cornerNRed.addRelativeAttractor(true, 0.4, -0.5, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Right, DriveState.Station)) ; } public static final ObjectList fieldBlueGeoFence = new ObjectList diff --git a/src/main/java/frc/robot/subsystems/AlgaeRoller.java b/src/main/java/frc/robot/subsystems/AlgaeRoller.java deleted file mode 100644 index 08f22b5..0000000 --- a/src/main/java/frc/robot/subsystems/AlgaeRoller.java +++ /dev/null @@ -1,47 +0,0 @@ -package frc.robot.subsystems; - -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.IDConstants; -import frc.robot.util.SD; - -public class AlgaeRoller extends SubsystemBase -{ - private TalonFX m_Algae; - private DigitalInput io_Sensor; - - public AlgaeRoller() - { - m_Algae = new TalonFX(IDConstants.AlgaeRollerID); - io_Sensor = new DigitalInput(IDConstants.AlgaeSensorDIO); - } - - public void setSpeed(double speed) - { - m_Algae.set(speed); - } - - public boolean getSensor() - { - return io_Sensor.get(); - } - - public Command runCommand(double speed) - { - return startEnd(() -> setSpeed(speed), () -> setSpeed(0)); - } - - public Command smartRunCommand(double speed) - { - return runCommand(speed).until(this::getSensor); - } - - @Override - public void periodic() - { - SD.ALGAE_ROLLER.put(m_Algae.get()); - } -} diff --git a/src/main/java/frc/robot/subsystems/CoralRoller.java b/src/main/java/frc/robot/subsystems/CoralRoller.java index 858494f..164ba58 100644 --- a/src/main/java/frc/robot/subsystems/CoralRoller.java +++ b/src/main/java/frc/robot/subsystems/CoralRoller.java @@ -6,7 +6,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.IDConstants; -import frc.robot.util.SD; public class CoralRoller extends SubsystemBase { @@ -19,29 +18,12 @@ public CoralRoller() io_Sensor = new DigitalInput(IDConstants.coralSensorDIO); } - public void setSpeed(double speed) - { - m_Coral.set(speed); - } - public boolean getSensor() - { - return io_Sensor.get(); - } + {return io_Sensor.get();} - public Command runCommand(double speed) - { - return startEnd(() -> setSpeed(speed), () -> setSpeed(0)); - } - - public Command smartRunCommand(double speed) - { - return runCommand(speed).until(this::getSensor); - } + public Command setSpeedCommand(double speed) + {return this.run(() -> m_Coral.set(speed));} @Override - public void periodic() - { - SD.CORAL_ROLLER.put(m_Coral.get()); - } + public void periodic() {} } diff --git a/src/main/java/frc/robot/subsystems/RumbleRequester.java b/src/main/java/frc/robot/subsystems/RumbleRequester.java index b213f87..d9a1a39 100644 --- a/src/main/java/frc/robot/subsystems/RumbleRequester.java +++ b/src/main/java/frc/robot/subsystems/RumbleRequester.java @@ -1,7 +1,6 @@ package frc.robot.subsystems; -import java.util.ArrayList; -import java.util.function.Consumer; +import java.util.HashSet; import java.util.function.DoubleSupplier; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -13,57 +12,45 @@ public class RumbleRequester extends SubsystemBase { - private ArrayList queue = new ArrayList(); + private HashSet queue = new HashSet(); private final CommandXboxController controller; private final RumbleType side; private final DoubleSupplier strengthSup; - private final Consumer displayCns; - public RumbleRequester(CommandXboxController controller, RumbleType side, Consumer displayCns, DoubleSupplier strengthSup) + public RumbleRequester(CommandXboxController controller, RumbleType side, DoubleSupplier strengthSup) { this.controller = controller; this.side = side; - this.displayCns = displayCns; this.strengthSup = strengthSup; } - private void addRequest(String requestID) - { - if(!queue.contains(requestID)) - {queue.add(requestID);} - } + private void add(String rumbleID) + {queue.add(rumbleID);} - private void removeRequest(String requestID) - {queue.remove(requestID);} + private void remove(String rumbleID) + {queue.remove(rumbleID);} + + public void clear() + {queue.clear();} - public Command requestCommand(boolean addRequest, String requestID) - {return addRequest ? Commands.runOnce(() -> addRequest(requestID)) : Commands.runOnce(() -> removeRequest(requestID));} + /** Returns a command that runs a rumble while it's running */ + public Command rumbleWhileCommand(String rumbleID) + {return Commands.startEnd(() -> add(rumbleID), () -> remove(rumbleID));} /** Adds a rumble with the provided ID that runs while the given trigger is true. Returns the subsystem for easier chaining. */ - public RumbleRequester addRumbleTrigger(String requestID, Trigger trigger) + public RumbleRequester addRumbleTrigger(String rumbleID, Trigger trigger) { - trigger.whileTrue(Commands.startEnd(() -> addRequest(requestID), () -> removeRequest(requestID))); + trigger.whileTrue(Commands.startEnd(() -> add(rumbleID), () -> remove(rumbleID))); return this; } - public void clearRequests() - {queue.clear();} - - public Command timedRequestCommand(String requestID, double durationSeconds) - { - return - Commands.sequence - ( - requestCommand(true, requestID), - Commands.waitSeconds(durationSeconds), - requestCommand(false, requestID) - ); - } + /** Returns a command that runs a rumble for the provided duration */ + public Command timedRequestCommand(String rumbleID, double durationSeconds) + {return rumbleWhileCommand(rumbleID).withDeadline(Commands.waitSeconds(durationSeconds));} @Override public void periodic() { controller.setRumble(side, queue.isEmpty() ? 0 : strengthSup.getAsDouble()); - displayCns.accept(queue.toString()); } } diff --git a/src/main/java/frc/robot/subsystems/vision/Limelight.java b/src/main/java/frc/robot/subsystems/vision/Limelight.java index 790e4c5..e5f1aa4 100644 --- a/src/main/java/frc/robot/subsystems/vision/Limelight.java +++ b/src/main/java/frc/robot/subsystems/vision/Limelight.java @@ -10,8 +10,8 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.Superstructure; -import frc.robot.util.LimelightHelpers.PoseEstimate; -import frc.robot.util.LimelightHelpers; +import frc.robot.util.libs.LimelightHelpers; +import frc.robot.util.libs.LimelightHelpers.PoseEstimate; public class Limelight { @@ -118,8 +118,6 @@ public void periodic() //RobotContainer.s_Swerve.resetPose(new Pose2d(RobotContainer.swerveState.Pose.getTranslation(), new Rotation2d(Math.toRadians(RobotContainer.s_Swerve.getPigeon2().getYaw().getValueAsDouble())))); } } - - //omegaRps = Units.radiansToRotations(RobotContainer.swerveState.Speeds.omegaRadiansPerSecond); } } } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 1fdcb0b..29536c5 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -24,8 +24,8 @@ import frc.robot.Superstructure; import frc.robot.constants.Constants; import frc.robot.subsystems.vision.Limelight.TagPOI; -import frc.robot.util.LimelightHelpers; import frc.robot.util.SD; +import frc.robot.util.libs.LimelightHelpers; public class Vision extends SubsystemBase { @@ -34,7 +34,7 @@ public class Vision extends SubsystemBase private final Limelight[] lls; private int[] validIDs = Constants.Vision.reefIDs; - private int pipelineIndex = (int)SD.IO_LL_EXPOSURE.defaultValue(); + private int pipelineIndex = (int)SD.LL_EXPOSURE.defaultValue(); private ArrayList rotationBuf = new ArrayList(); private boolean lastCycleRotationKnown = false; @@ -64,14 +64,14 @@ public void incrementPipeline() { pipelineIndex = MathUtil.clamp(pipelineIndex + 1, 0, 7); for (var ll : lls) {ll.updatePipeline(pipelineIndex);} - SD.IO_LL_EXPOSURE.put(pipelineIndex); + SD.LL_EXPOSURE.put(pipelineIndex); } public void decrementPipeline() { pipelineIndex = MathUtil.clamp(pipelineIndex - 1, 0, 7); for (var ll : lls) {ll.updatePipeline(pipelineIndex);} - SD.IO_LL_EXPOSURE.put(pipelineIndex); + SD.LL_EXPOSURE.put(pipelineIndex); } @Override @@ -82,7 +82,7 @@ public void periodic() ll.updateValidIDs(validIDs); } - if (SD.IO_LL.get()) + if (SD.LL_TOGGLE.get()) { for (var ll : lls) { diff --git a/src/main/java/frc/robot/util/AutoFactories.java b/src/main/java/frc/robot/util/AutoFactories.java index 57395a6..85f2078 100644 --- a/src/main/java/frc/robot/util/AutoFactories.java +++ b/src/main/java/frc/robot/util/AutoFactories.java @@ -7,7 +7,6 @@ import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; @@ -30,7 +29,7 @@ public class AutoFactories */ public static Command getCommandList(String commandInput, CoralRoller s_Coral, CommandSwerveDrivetrain s_Swerve, Supplier swerveStateSup) { - // Removes all space characters from the single-String command phrases, ensures it's all lowercase, and then splits it into individual strings, which are stored in an array + // Removes all whitespace characters from the single-String command phrases, ensures it's all lowercase, and then splits it into individual strings, which are stored in an array String[] splitCommands = commandInput.replaceAll("//s", "").toLowerCase().split(","); // The arraylist that all the commands will be placed into ArrayList commandList = new ArrayList<>(); @@ -71,7 +70,7 @@ public static Command getCommandList(String commandInput, CoralRoller s_Coral, C case 'r': commandList.add(s_Swerve.poseLockDriveCommand(new AlliancePose2dSup(FieldConstants.getLineup(splitCommand)), swerveStateSup)); commandList.add(Commands.waitSeconds(0.1)); - commandList.add(s_Coral.smartRunCommand(Constants.Coral.forwardSpeed)); + commandList.add(s_Coral.setSpeedCommand(Constants.Coral.forwardSpeed).until(s_Coral::getSensor)); break; case 'c': @@ -86,16 +85,4 @@ public static Command getCommandList(String commandInput, CoralRoller s_Coral, C return new SequentialCommandGroup(commandList.toArray(Command[]::new)).withInterruptBehavior(InterruptionBehavior.kCancelIncoming); } - - public static void displayPose(Pose2d pose, Rotation2d rotation) - { - SD.IO_POSE_X.put(pose.getX()); - SD.IO_POSE_Y.put(pose.getY()); - SD.IO_POSE_R.put(rotation.getDegrees()); - } - - public static void displayPose(Pose2d pose) - { - displayPose(pose, pose.getRotation()); - } } diff --git a/src/main/java/frc/robot/util/SD.java b/src/main/java/frc/robot/util/SD.java index e103470..2e8ed90 100644 --- a/src/main/java/frc/robot/util/SD.java +++ b/src/main/java/frc/robot/util/SD.java @@ -4,8 +4,6 @@ package frc.robot.util; -import java.util.Set; - import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -15,60 +13,32 @@ /** Simplified interface for most SmartDashboard interactions */ public class SD { - public static final BooleanKey IO_LL = new BooleanKey("Use Limelight", true); - public static final DoubleKey IO_LL_EXPOSURE = new DoubleKey("Exposure Setting", 0); - public static final BooleanKey IO_LL_EXPOSURE_UP = new BooleanKey("Increase Exposure", false); - public static final BooleanKey IO_LL_EXPOSURE_DOWN = new BooleanKey("Decrease Exposure", false); - - public static final StringKey STATE_HEADING = new StringKey("Heading State", ""); - public static final StringKey STATE_DRIVE = new StringKey("Drive State", "Disabled"); - public static final BooleanKey STATE_HEADING_SNAP = new BooleanKey("Heading Snap Updating", true); - - public static final BooleanKey IO_GEOFENCE = new BooleanKey("Use Fence", true); - public static final BooleanKey IO_OUTER_GEOFENCE = new BooleanKey("Wall Fence", true); - public static final DoubleKey IO_GEOFENCE_IMPACT = new DoubleKey("Fence Impact", 1); - public static final DoubleKey IO_RUMBLE_D = new DoubleKey("Driver Rumble", Constants.RumblerConstants.driverDefault); - public static final DoubleKey IO_RUMBLE_C = new DoubleKey("Copilot Rumble", Constants.RumblerConstants.copilotDefault); - - public static final DoubleKey SENSOR_GYRO = new DoubleKey("Gyro yaw", 0); + public static final StringKey AUTO_STRING = new StringKey("Auto String", ""); - public static final StringKey RUMBLE_D_R = new StringKey("DriverRight Rumble Queue", ""); - public static final StringKey RUMBLE_D_L = new StringKey("DriverLeft Rumble Queue", ""); - public static final StringKey RUMBLE_C_R = new StringKey("CopilotRight Rumble Queue", ""); - public static final StringKey RUMBLE_C_L = new StringKey("CopilotLeft Rumble Queue", ""); + public static final DoubleKey LL_EXPOSURE = new DoubleKey("Exposure Setting", 0); + public static final BooleanKey LL_EXPOSURE_UP = new BooleanKey("Increase Exposure", false); + public static final BooleanKey LL_EXPOSURE_DOWN = new BooleanKey("Decrease Exposure", false); + public static final BooleanKey LL_TOGGLE = new BooleanKey("Use Limelight", true); - public static final DoubleKey IO_POSE_X = new DoubleKey("Pose X", 0.0); - public static final DoubleKey IO_POSE_Y = new DoubleKey("Pose Y", 0.0); - public static final DoubleKey IO_POSE_R = new DoubleKey("Pose Rotation", 0.0); - public static final StringKey IO_AUTO = new StringKey("Auto String", ""); + public static final StringKey STATE_HEADING = new StringKey("Heading State", ""); + public static final StringKey STATE_DRIVE = new StringKey("Drive State", "Disabled"); - public static final DoubleKey IO_CORALSPEED_F = new DoubleKey("Coral Roller Forward Speed", Constants.Coral.forwardSpeed); - public static final DoubleKey IO_CORALSPEED_R = new DoubleKey("Coral Roller Reverse Speed", Constants.Coral.reverseSpeed); - public static final DoubleKey IO_ALGAESPEED_F = new DoubleKey("Algae Roller Forward Speed", Constants.Algae.forwardSpeed); - public static final DoubleKey IO_ALGAESPEED_R = new DoubleKey("Algae Roller Reverse Speed", Constants.Algae.reverseSpeed); - - public static final DoubleKey CORAL_ROLLER = new DoubleKey("Current Coral Roller Speed", 0); - public static final DoubleKey ALGAE_ROLLER = new DoubleKey("Current Algae Roller Speed", 0); + public static final DoubleKey RUMBLE_DRIVER = new DoubleKey("Driver Rumble", Constants.RumblerConstants.driverDefault); + public static final DoubleKey RUMBLE_OPERATOR = new DoubleKey("Operator Rumble", Constants.RumblerConstants.operatorDefault); static { - for - ( - Initable key : - Set.of - ( - IO_POSE_X, - IO_POSE_Y, - IO_POSE_R, - IO_GEOFENCE, - IO_AUTO, - IO_CORALSPEED_F, - IO_CORALSPEED_R - ) - ) - { - key.init(); - } + try { + Class thisClass = Class.forName("frc.robot.util.SD"); + var fields = thisClass.getFields(); + for (var field : fields) + { + if (field.get(null) instanceof Key key) + { + key.init(); + } + } + } catch (Exception e) {/* This will verifiably never happen */} } public static void initSwerveDisplay(CommandSwerveDrivetrain s_Swerve) @@ -102,14 +72,18 @@ public void initSendable(SendableBuilder builder) ); } - public interface Initable + private interface Key { + public T get(); + + public void put(T value); + public void init(); } - public record BooleanKey (String label, boolean defaultValue) implements Initable + public record BooleanKey (String label, boolean defaultValue) implements Key { - public boolean get() {return SmartDashboard.getBoolean(label, defaultValue);} + public Boolean get() {return SmartDashboard.getBoolean(label, defaultValue);} public boolean button() { @@ -124,19 +98,19 @@ public boolean button() public void init() {SmartDashboard.putBoolean(label, defaultValue);} - public void put(boolean value) {SmartDashboard.putBoolean(label, value);} + public void put(Boolean value) {SmartDashboard.putBoolean(label, value);} } - public record DoubleKey (String label, double defaultValue) implements Initable + public record DoubleKey (String label, double defaultValue) implements Key { public Double get() {return SmartDashboard.getNumber(label, defaultValue);} public void init() {SmartDashboard.putNumber(label, defaultValue);} - public void put(double value) {SmartDashboard.putNumber(label, value);} + public void put(Double value) {SmartDashboard.putNumber(label, value);} } - public record StringKey (String label, String defaultValue) implements Initable + public record StringKey (String label, String defaultValue) implements Key { public String get() {return SmartDashboard.getString(label, defaultValue);} diff --git a/src/main/java/frc/robot/util/LimelightHelpers.java b/src/main/java/frc/robot/util/libs/LimelightHelpers.java similarity index 99% rename from src/main/java/frc/robot/util/LimelightHelpers.java rename to src/main/java/frc/robot/util/libs/LimelightHelpers.java index b6431e6..d4d6fd1 100644 --- a/src/main/java/frc/robot/util/LimelightHelpers.java +++ b/src/main/java/frc/robot/util/libs/LimelightHelpers.java @@ -1,6 +1,6 @@ //LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER) -package frc.robot.util; +package frc.robot.util.libs; import edu.wpi.first.networktables.DoubleArrayEntry; import edu.wpi.first.networktables.NetworkTable; diff --git a/src/main/java/frc/robot/util/Telemetry.java b/src/main/java/frc/robot/util/libs/Telemetry.java similarity index 99% rename from src/main/java/frc/robot/util/Telemetry.java rename to src/main/java/frc/robot/util/libs/Telemetry.java index 4d1296c..cb7d226 100644 --- a/src/main/java/frc/robot/util/Telemetry.java +++ b/src/main/java/frc/robot/util/libs/Telemetry.java @@ -1,4 +1,4 @@ -package frc.robot.util; +package frc.robot.util.libs; import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; From 714cface30daecbfbf9e7332ebf9b9c7803fe764 Mon Sep 17 00:00:00 2001 From: ThePixelatedCat Date: Tue, 19 Aug 2025 18:14:21 +1000 Subject: [PATCH 05/13] Cleaned up SD reflection implementation --- src/main/java/frc/robot/util/SD.java | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/util/SD.java b/src/main/java/frc/robot/util/SD.java index 2e8ed90..3108299 100644 --- a/src/main/java/frc/robot/util/SD.java +++ b/src/main/java/frc/robot/util/SD.java @@ -28,15 +28,11 @@ public class SD static { - try { - Class thisClass = Class.forName("frc.robot.util.SD"); - var fields = thisClass.getFields(); - for (var field : fields) + try + { + for (var field : SD.class.getFields()) { - if (field.get(null) instanceof Key key) - { - key.init(); - } + if (field.get(null) instanceof Key key) key.init(); } } catch (Exception e) {/* This will verifiably never happen */} } From 044cea3981ad0d19c16170d65061992856296e93 Mon Sep 17 00:00:00 2001 From: ThePixelatedCat Date: Tue, 19 Aug 2025 18:57:12 +1000 Subject: [PATCH 06/13] Moved TagPOI from Limelight to Vision, removed unneeded things from Limelight, started tidying up vision rotation fetching --- src/main/java/frc/robot/Superstructure.java | 2 +- .../java/frc/robot/constants/Constants.java | 8 +- .../java/frc/robot/constants/IDConstants.java | 3 - .../robot/subsystems/vision/Limelight.java | 108 +++-------------- .../frc/robot/subsystems/vision/Vision.java | 109 +++++++++--------- 5 files changed, 75 insertions(+), 155 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 9f4a52f..fa95285 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -23,7 +23,7 @@ import frc.robot.constants.*; import frc.robot.subsystems.*; import frc.robot.subsystems.vision.*; -import frc.robot.subsystems.vision.Limelight.TagPOI; +import frc.robot.subsystems.vision.Vision.TagPOI; import frc.robot.util.AutoFactories; import frc.robot.util.FieldUtils; import frc.robot.util.SD; diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 00d1f33..3d08818 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -70,12 +70,6 @@ public static final class Coral public static final double reverseSpeed = 0.15; } - public static final class Algae - { - public static final double forwardSpeed = 0.5; - public static final double reverseSpeed = -0.5; - } - public static final class Vision { /* public static final int[] validIDs = @@ -112,5 +106,7 @@ public static final class Vision public static final double linearStdDevBaseline = 0.08; /** Baseline 1 meter, 1 tag stddev rotation, in radians */ public static final double rotStdDevBaseline = 999; + /** How many good MT1 readings to get before setting rotation and moving to MT2 */ + public static final int mt1CyclesNeeded = 10; } } diff --git a/src/main/java/frc/robot/constants/IDConstants.java b/src/main/java/frc/robot/constants/IDConstants.java index 31537c6..64e30a4 100644 --- a/src/main/java/frc/robot/constants/IDConstants.java +++ b/src/main/java/frc/robot/constants/IDConstants.java @@ -10,7 +10,6 @@ public final class IDConstants /* Drive */ /* _____ */ - public static final int foreStbdDriveMotorID = 1; public static final int foreStbdAngleMotorID = 2; public static final int foreStbdCANcoderID = 3; @@ -31,11 +30,9 @@ public final class IDConstants /* Mechanism */ /* _________ */ - public static final int coralRollerID = 13; public static final int coralSensorDIO = 0; public static final int AlgaeRollerID = 14; public static final int AlgaeSensorDIO = 1; - } diff --git a/src/main/java/frc/robot/subsystems/vision/Limelight.java b/src/main/java/frc/robot/subsystems/vision/Limelight.java index e5f1aa4..af89047 100644 --- a/src/main/java/frc/robot/subsystems/vision/Limelight.java +++ b/src/main/java/frc/robot/subsystems/vision/Limelight.java @@ -4,120 +4,44 @@ package frc.robot.subsystems.vision; -import java.util.ArrayList; +import java.util.Optional; -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.Superstructure; + import frc.robot.util.libs.LimelightHelpers; import frc.robot.util.libs.LimelightHelpers.PoseEstimate; public class Limelight -{ - private LimelightHelpers.PoseEstimate mt2; - private LimelightHelpers.PoseEstimate mt1; - - private final String limelightName; - - public static boolean rotationKnown = false; - private ArrayList rotationData = new ArrayList(); - private boolean lastCycleRotationKnown = false; - private final int mt1CyclesNeeded = 10; - - public enum TagPOI - { - REEF, - BARGE, - PROCESSOR, - CORALSTATION - } +{ + private final String name; /** Creates a new Limelight. */ public Limelight(String name) - { - limelightName = name; - } + {this.name = name;} public void setIMUMode(int mode) - {LimelightHelpers.SetIMUMode(limelightName, mode);} + {LimelightHelpers.SetIMUMode(name, mode);} - public Rotation2d getLimelightRotation() + public Optional getLimelightRotation() { - mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName); + var mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(name); - if (mt1 != null && mt1.avgTagDist < 4) - {return mt1.pose.getRotation();} - return Rotation2d.kZero; + return (mt1 != null && mt1.avgTagDist < 4) ? + Optional.of(mt1.pose.getRotation()) : + Optional.empty(); } protected void updateValidIDs(int[] validIDs) - { - LimelightHelpers.SetFiducialIDFiltersOverride(limelightName, validIDs); - } + {LimelightHelpers.SetFiducialIDFiltersOverride(name, validIDs);} protected void updatePipeline(int pipelineIndex) - { - LimelightHelpers.setPipelineIndex(limelightName, pipelineIndex); - } - - @Logged - public Pose3d getMT1Pose() - {return LimelightHelpers.getBotPose3d_wpiBlue(limelightName);} + {LimelightHelpers.setPipelineIndex(name, pipelineIndex);} public PoseEstimate getMT2(double headingDeg) { - LimelightHelpers.SetRobotOrientation(limelightName, headingDeg, 0, 0, 0, 0, 0); - return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName); + LimelightHelpers.SetRobotOrientation(name, headingDeg, 0, 0, 0, 0, 0); + return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name); } - public void periodic() - { - if (Superstructure.isVisionActive()) - { - rotationKnown = Superstructure.isRotationKnown(); - - if (!rotationKnown) - { - lastCycleRotationKnown = false; - if (!getLimelightRotation().equals(Rotation2d.kZero)) - { - rotationData.add(0, getLimelightRotation().getDegrees()); - - if (rotationData.size() > mt1CyclesNeeded) - {rotationData.remove(mt1CyclesNeeded);} - - if (rotationData.size() == mt1CyclesNeeded) - { - double lowest = rotationData.get(0).doubleValue(); - double highest = rotationData.get(0).doubleValue(); - - for(int i = 1; i < mt1CyclesNeeded; i++) - { - lowest = Math.min(lowest, rotationData.get(i).doubleValue()); - highest = Math.max(highest, rotationData.get(i).doubleValue()); - } - - if (highest - lowest < 1) - { - rotationKnown = true; - Superstructure.setRotationKnown(true); - //SmartDashboard.putNumber("limelight " + limelightName + " average rotation reading", (highest + lowest) / 2); - Superstructure.setYaw((highest + lowest) / 2); - } - } - } - } - - if (!lastCycleRotationKnown) - { - if (rotationKnown) - { - rotationData.clear(); - lastCycleRotationKnown = true; - //RobotContainer.s_Swerve.resetPose(new Pose2d(RobotContainer.swerveState.Pose.getTranslation(), new Rotation2d(Math.toRadians(RobotContainer.s_Swerve.getPigeon2().getYaw().getValueAsDouble())))); - } - } - } - } + public void periodic() {} } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 29536c5..1e23a7e 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -21,77 +21,74 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.SubsystemBase; + import frc.robot.Superstructure; -import frc.robot.constants.Constants; -import frc.robot.subsystems.vision.Limelight.TagPOI; import frc.robot.util.SD; import frc.robot.util.libs.LimelightHelpers; +import static frc.robot.constants.Constants.Vision.*; public class Vision extends SubsystemBase { - private final PoseEstimateConsumer consumer; - private final Supplier> rotationData; + public enum TagPOI {REEF, BARGE, PROCESSOR, CORALSTATION} + + private final PoseEstimateConsumer estimateConsumer; + private final Supplier> rotationDataSup; private final Limelight[] lls; - private int[] validIDs = Constants.Vision.reefIDs; private int pipelineIndex = (int)SD.LL_EXPOSURE.defaultValue(); private ArrayList rotationBuf = new ArrayList(); + private boolean rotationKnown = false; private boolean lastCycleRotationKnown = false; - private final int mt1CyclesNeeded = 10; /** Creates a new Vision. */ - public Vision(PoseEstimateConsumer consumer, Supplier> rotationData, Limelight... lls) + public Vision(PoseEstimateConsumer estimateConsumer, Supplier> rotationDataSup, Limelight... lls) { - this.consumer = consumer; - this.rotationData = rotationData; + this.estimateConsumer = estimateConsumer; + this.rotationDataSup = rotationDataSup; this.lls = lls; + setActivePOI(TagPOI.REEF); } public void setActivePOI(TagPOI activePOI) { - validIDs = - switch (activePOI) + var validIDs = switch (activePOI) { - case REEF -> Constants.Vision.reefIDs; - case BARGE -> Constants.Vision.bargeIDs; - case CORALSTATION, PROCESSOR -> Constants.Vision.humanPlayerStationIDs; - default -> Constants.Vision.reefIDs; + case REEF -> reefIDs; + case BARGE -> bargeIDs; + case CORALSTATION, PROCESSOR -> humanPlayerStationIDs; + default -> reefIDs; }; + + for (var ll : lls) ll.updateValidIDs(validIDs); } public void incrementPipeline() { pipelineIndex = MathUtil.clamp(pipelineIndex + 1, 0, 7); for (var ll : lls) {ll.updatePipeline(pipelineIndex);} - SD.LL_EXPOSURE.put(pipelineIndex); + SD.LL_EXPOSURE.put((double)pipelineIndex); } public void decrementPipeline() { pipelineIndex = MathUtil.clamp(pipelineIndex - 1, 0, 7); for (var ll : lls) {ll.updatePipeline(pipelineIndex);} - SD.LL_EXPOSURE.put(pipelineIndex); + SD.LL_EXPOSURE.put((double)pipelineIndex); } @Override public void periodic() { - for (var ll : lls) - { - ll.updateValidIDs(validIDs); - } - if (SD.LL_TOGGLE.get()) { for (var ll : lls) { - var rotationVals = rotationData.get(); - double heading = rotationVals.getFirst(); - double omegaRps = rotationVals.getSecond(); + var rotationData = rotationDataSup.get(); + double heading = rotationData.getFirst(); + double omegaRps = rotationData.getSecond(); var mt2 = ll.getMT2(heading); - //mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName); boolean useUpdate = !(mt2 == null || mt2.tagCount == 0 || omegaRps > 2.0); @@ -99,47 +96,53 @@ public void periodic() { double stdDevFactor = Math.pow(mt2.avgTagDist, 2.0) / mt2.tagCount; - double linearStdDev = Constants.Vision.linearStdDevBaseline * stdDevFactor; - double rotStdDev = Constants.Vision.rotStdDevBaseline * stdDevFactor; + double linearStdDev = linearStdDevBaseline * stdDevFactor; + double rotStdDev = rotStdDevBaseline * stdDevFactor; - consumer.accept(mt2.pose, Utils.fpgaToCurrentTime(mt2.timestampSeconds), VecBuilder.fill(linearStdDev, linearStdDev, rotStdDev)); + estimateConsumer.accept(mt2.pose, Utils.fpgaToCurrentTime(mt2.timestampSeconds), VecBuilder.fill(linearStdDev, linearStdDev, rotStdDev)); } } } if (Superstructure.isVisionActive()) { - boolean rotationKnown = Superstructure.isRotationKnown(); + rotationKnown = Superstructure.isRotationKnown(); if (!rotationKnown) { lastCycleRotationKnown = false; - if (!getLimelightRotation().equals(Rotation2d.kZero)) + + for (var ll : lls) { - rotationBuf.add(0, getLimelightRotation().getDegrees()); - - if (rotationBuf.size() > mt1CyclesNeeded) - {rotationBuf.remove(mt1CyclesNeeded);} - - if (rotationBuf.size() == mt1CyclesNeeded) - { - double lowest = rotationBuf.get(0).doubleValue(); - double highest = rotationBuf.get(0).doubleValue(); - - for(int i = 1; i < mt1CyclesNeeded; i++) + ll.getLimelightRotation().ifPresent + ( + rotationReading -> { - lowest = Math.min(lowest, rotationBuf.get(i).doubleValue()); - highest = Math.max(highest, rotationBuf.get(i).doubleValue()); - } - - if (highest - lowest < 1) - { - rotationKnown = true; - Superstructure.setRotationKnown(true); - //SmartDashboard.putNumber("limelight " + limelightName + " average rotation reading", (highest + lowest) / 2); - Superstructure.setYaw((highest + lowest) / 2); + rotationBuf.add(0, rotationReading.getDegrees()); + + if (rotationBuf.size() > mt1CyclesNeeded) + {rotationBuf.remove(mt1CyclesNeeded);} + + if (rotationBuf.size() == mt1CyclesNeeded) + { + double lowest = rotationBuf.get(0).doubleValue(); + double highest = rotationBuf.get(0).doubleValue(); + + for(var reading : rotationBuf) + { + lowest = Math.min(lowest, reading.doubleValue()); + highest = Math.max(highest, reading.doubleValue()); + } + + if (highest - lowest < 1) + { + rotationKnown = true; + Superstructure.setRotationKnown(true); + Superstructure.setYaw((highest + lowest) / 2); + } + } } - } + ); } } From 3295cbe87344f2cc7ee17eb7b9aaa1dd00cd5570 Mon Sep 17 00:00:00 2001 From: ThePixelatedCat Date: Wed, 20 Aug 2025 13:13:22 +1000 Subject: [PATCH 07/13] Modern syntax for AutoBuilder switch, implemented supplier and consumer for Keys, removed unused methods in Robot --- src/main/java/frc/robot/Robot.java | 32 +------------------ .../java/frc/robot/util/AutoFactories.java | 28 ++++++++-------- src/main/java/frc/robot/util/SD.java | 4 ++- 3 files changed, 17 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5ac79ae..cba39a6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -38,15 +38,6 @@ public void robotPeriodic() CommandScheduler.getInstance().run(); } - @Override - public void disabledInit() {} - - @Override - public void disabledPeriodic() {} - - @Override - public void disabledExit() {} - @Override public void autonomousInit() { @@ -55,12 +46,6 @@ public void autonomousInit() if (m_autonomousCommand != null) m_autonomousCommand.schedule(); } - @Override - public void autonomousPeriodic() {} - - @Override - public void autonomousExit() {} - @Override public void teleopInit() { @@ -68,20 +53,5 @@ public void teleopInit() } @Override - public void teleopPeriodic() {} - - @Override - public void teleopExit() {} - - @Override - public void testInit() - { - CommandScheduler.getInstance().cancelAll(); - } - - @Override - public void testPeriodic() {} - - @Override - public void testExit() {} + public void testInit() {CommandScheduler.getInstance().cancelAll();} } diff --git a/src/main/java/frc/robot/util/AutoFactories.java b/src/main/java/frc/robot/util/AutoFactories.java index 85f2078..24862d8 100644 --- a/src/main/java/frc/robot/util/AutoFactories.java +++ b/src/main/java/frc/robot/util/AutoFactories.java @@ -39,7 +39,8 @@ public static Command getCommandList(String commandInput, CoralRoller s_Coral, C { switch (splitCommand.charAt(0)) { - case 'g': + case 'g' -> + { int seperatorIndex = splitCommand.indexOf(":"); Translation2d posTarget = @@ -55,31 +56,28 @@ public static Command getCommandList(String commandInput, CoralRoller s_Coral, C swerveStateSup.get().Pose.getRotation().plus(Rotation2d.k180deg); commandList.add(s_Swerve.poseLockDriveCommand(new AlliancePose2dSup(posTarget, rotationTarget), swerveStateSup)); - break; + } - case 'w': - commandList.add(Commands.waitSeconds(Double.parseDouble(splitCommand.substring(1)))); - break; + case 'w' -> commandList.add(Commands.waitSeconds(Double.parseDouble(splitCommand.substring(1)))); - case 't': + case 't' -> + { double targetMatchTimeElapsed = Double.parseDouble(splitCommand.substring(1)); - commandList.add(Commands.waitUntil(() -> Timer.getMatchTime() < (15 - targetMatchTimeElapsed))); - break; + } - case 'r': + case 'r' -> + { commandList.add(s_Swerve.poseLockDriveCommand(new AlliancePose2dSup(FieldConstants.getLineup(splitCommand)), swerveStateSup)); commandList.add(Commands.waitSeconds(0.1)); commandList.add(s_Coral.setSpeedCommand(Constants.Coral.forwardSpeed).until(s_Coral::getSensor)); - break; + } - case 'c': + case 'c' -> + { commandList.add(s_Swerve.poseLockDriveCommand(new AlliancePose2dSup(FieldConstants.getLineup(splitCommand)), swerveStateSup)); commandList.add(Commands.waitUntil(s_Coral::getSensor)); - break; - - default: - break; + } } } diff --git a/src/main/java/frc/robot/util/SD.java b/src/main/java/frc/robot/util/SD.java index 3108299..773efdb 100644 --- a/src/main/java/frc/robot/util/SD.java +++ b/src/main/java/frc/robot/util/SD.java @@ -68,8 +68,10 @@ public void initSendable(SendableBuilder builder) ); } - private interface Key + private interface Key extends Supplier, Consumer { + public void accept(T value) {put(value);} + public T get(); public void put(T value); From bdbd797f80d7f7f24969c4ea7ff55ba3a84bbda2 Mon Sep 17 00:00:00 2001 From: ThePixelatedCat Date: Wed, 20 Aug 2025 13:16:21 +1000 Subject: [PATCH 08/13] Fixed bugs with Supplier and Consumer implementation for Key, adjusted other things to use those implementations --- src/main/java/frc/robot/Superstructure.java | 8 ++++---- src/main/java/frc/robot/subsystems/RumbleRequester.java | 8 ++++---- src/main/java/frc/robot/util/SD.java | 5 ++++- 3 files changed, 12 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index fa95285..a1391e2 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -54,10 +54,10 @@ public enum DriveState {Reef, Station, Barge, None} private final CommandXboxController driver = new CommandXboxController(0); private final CommandXboxController operator = new CommandXboxController(1); - private final RumbleRequester io_driverRight = new RumbleRequester(driver, RumbleType.kRightRumble, SD.RUMBLE_DRIVER::get); - private final RumbleRequester io_driverLeft = new RumbleRequester(driver, RumbleType.kLeftRumble, SD.RUMBLE_DRIVER::get); - private final RumbleRequester io_operatorRight = new RumbleRequester(operator, RumbleType.kRightRumble, SD.RUMBLE_OPERATOR::get); - private final RumbleRequester io_operatorLeft = new RumbleRequester(operator, RumbleType.kLeftRumble, SD.RUMBLE_OPERATOR::get); + private final RumbleRequester io_driverRight = new RumbleRequester(driver, RumbleType.kRightRumble, SD.RUMBLE_DRIVER); + private final RumbleRequester io_driverLeft = new RumbleRequester(driver, RumbleType.kLeftRumble, SD.RUMBLE_DRIVER); + private final RumbleRequester io_operatorRight = new RumbleRequester(operator, RumbleType.kRightRumble, SD.RUMBLE_OPERATOR); + private final RumbleRequester io_operatorLeft = new RumbleRequester(operator, RumbleType.kLeftRumble, SD.RUMBLE_OPERATOR); private final JoystickTransmuter driverStick = new JoystickTransmuter(driver::getLeftY, driver::getLeftX).invertX().invertY(); private final Brake driverBrake = new Brake(driver::getRightTriggerAxis, Constants.Control.maxThrottle, Constants.Control.minThrottle); diff --git a/src/main/java/frc/robot/subsystems/RumbleRequester.java b/src/main/java/frc/robot/subsystems/RumbleRequester.java index d9a1a39..d013e3e 100644 --- a/src/main/java/frc/robot/subsystems/RumbleRequester.java +++ b/src/main/java/frc/robot/subsystems/RumbleRequester.java @@ -1,7 +1,7 @@ package frc.robot.subsystems; import java.util.HashSet; -import java.util.function.DoubleSupplier; +import java.util.function.Supplier; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.Command; @@ -15,9 +15,9 @@ public class RumbleRequester extends SubsystemBase private HashSet queue = new HashSet(); private final CommandXboxController controller; private final RumbleType side; - private final DoubleSupplier strengthSup; + private final Supplier strengthSup; - public RumbleRequester(CommandXboxController controller, RumbleType side, DoubleSupplier strengthSup) + public RumbleRequester(CommandXboxController controller, RumbleType side, Supplier strengthSup) { this.controller = controller; this.side = side; @@ -51,6 +51,6 @@ public Command timedRequestCommand(String rumbleID, double durationSeconds) @Override public void periodic() { - controller.setRumble(side, queue.isEmpty() ? 0 : strengthSup.getAsDouble()); + controller.setRumble(side, queue.isEmpty() ? 0 : strengthSup.get()); } } diff --git a/src/main/java/frc/robot/util/SD.java b/src/main/java/frc/robot/util/SD.java index 773efdb..1d7767f 100644 --- a/src/main/java/frc/robot/util/SD.java +++ b/src/main/java/frc/robot/util/SD.java @@ -4,6 +4,9 @@ package frc.robot.util; +import java.util.function.Consumer; +import java.util.function.Supplier; + import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -70,7 +73,7 @@ public void initSendable(SendableBuilder builder) private interface Key extends Supplier, Consumer { - public void accept(T value) {put(value);} + public default void accept(T value) {put(value);} public T get(); From d61970c733b603cd3830c2e7ab7fc57289fc4cc3 Mon Sep 17 00:00:00 2001 From: ThePixelatedCat Date: Wed, 20 Aug 2025 13:39:20 +1000 Subject: [PATCH 09/13] Changed PID to pose to end once we're at the target, removed unneeded pathplanner remnants --- src/main/java/frc/robot/Superstructure.java | 4 +- .../java/frc/robot/constants/Constants.java | 3 ++ .../subsystems/CommandSwerveDrivetrain.java | 19 ++++------ .../java/frc/robot/util/AutoFactories.java | 6 +-- src/main/java/frc/robot/util/FieldUtils.java | 23 +++-------- vendordeps/PathplannerLib-2025.2.7.json | 38 ------------------- 6 files changed, 22 insertions(+), 71 deletions(-) delete mode 100644 vendordeps/PathplannerLib-2025.2.7.json diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index a1391e2..91859ac 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -232,13 +232,13 @@ private void bindControls() driver.b().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.None)); driver.axisMagnitudeGreaterThan(Axis.kRightX.value, 0.2).onTrue(Commands.runOnce(() -> currentDriveState = DriveState.None)); - new Trigger(() -> FieldUtils.atReefLineUp(swerveState.Pose.getTranslation())).whileTrue(s_Coral.setSpeedCommand(Constants.Coral.forwardSpeed)); + new Trigger(() -> FieldUtils.atReefLineUp(swerveState.Pose)).whileTrue(s_Coral.setSpeedCommand(Constants.Coral.forwardSpeed)); } private void bindRumbles() { io_operatorLeft.addRumbleTrigger("CoralHeld", new Trigger(s_Coral::getSensor)); - io_operatorRight.addRumbleTrigger("ScoreReady" , new Trigger(() -> FieldUtils.atReefLineUp(swerveState.Pose.getTranslation()))); + io_operatorRight.addRumbleTrigger("ScoreReady" , new Trigger(() -> FieldUtils.atReefLineUp(swerveState.Pose))); } private void bindSD() diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 3d08818..8685480 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -33,7 +33,10 @@ public static final class Control public static final double manualClimberScale = 1; public static final double driveSnappingRange = 1.5; public static final double cageFaceDistance = 1.5; + /** Translation lineup tolerance, in meters */ public static final double lineupTolerance = 0.05; + /** Rotation lineup tolerance, in degrees */ + public static final double angleLineupTolerance = 1.5; } public static final class Swerve diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 2abfb05..37fe995 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -27,8 +27,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.constants.Constants; +import static frc.robot.constants.Constants.Swerve.*; import frc.robot.constants.TunerConstants.TunerSwerveDrivetrain; +import frc.robot.util.FieldUtils; import frc.robot.util.controlTransmutation.PIDDriveTransmuter; /** @@ -37,8 +38,8 @@ */ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem, Sendable { - private final PIDController thetaController = new PIDController(Constants.Swerve.rotationKP, Constants.Swerve.rotationKI, Constants.Swerve.rotationKD); - private final PIDDriveTransmuter pidTransmuter = new PIDDriveTransmuter(Constants.Swerve.driveKP, Constants.Swerve.driveKI, Constants.Swerve.driveKD); + private final PIDController thetaController = new PIDController(rotationKP, rotationKI, rotationKD); + private final PIDDriveTransmuter pidTransmuter = new PIDDriveTransmuter(driveKP, driveKI, driveKD); private static final double kSimLoopPeriod = 0.005; // 5 ms private Notifier m_simNotifier = null; @@ -265,12 +266,9 @@ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) public Command sysIdDynamic(SysIdRoutine.Direction direction) {return m_sysIdRoutineToApply.dynamic(direction);} - public Command poseLockDriveCommand(Supplier targetSupplier, Supplier swerveStateSup) + public Command poseDriveCommand(Supplier targetSupplier, Supplier swerveStateSup) { - final double maxSpeed = Constants.Swerve.maxSpeed; - - final var driveRequest = new SwerveRequest - .ApplyRobotSpeeds(); + final var driveRequest = new SwerveRequest.ApplyRobotSpeeds(); pidTransmuter.withTargetPoseSup(targetSupplier); @@ -281,7 +279,7 @@ public Command poseLockDriveCommand(Supplier targetSupplier, Supplier targetSupplier, Supplier FieldUtils.atPose(swerveStateSup.get().Pose, targetSupplier.get())); } @Override diff --git a/src/main/java/frc/robot/util/AutoFactories.java b/src/main/java/frc/robot/util/AutoFactories.java index 24862d8..f7cd516 100644 --- a/src/main/java/frc/robot/util/AutoFactories.java +++ b/src/main/java/frc/robot/util/AutoFactories.java @@ -55,7 +55,7 @@ public static Command getCommandList(String commandInput, CoralRoller s_Coral, C new Rotation2d(Units.degreesToRadians(Double.parseDouble(splitCommand.substring(splitCommand.indexOf(";"))))) : swerveStateSup.get().Pose.getRotation().plus(Rotation2d.k180deg); - commandList.add(s_Swerve.poseLockDriveCommand(new AlliancePose2dSup(posTarget, rotationTarget), swerveStateSup)); + commandList.add(s_Swerve.poseDriveCommand(new AlliancePose2dSup(posTarget, rotationTarget), swerveStateSup)); } case 'w' -> commandList.add(Commands.waitSeconds(Double.parseDouble(splitCommand.substring(1)))); @@ -68,14 +68,14 @@ public static Command getCommandList(String commandInput, CoralRoller s_Coral, C case 'r' -> { - commandList.add(s_Swerve.poseLockDriveCommand(new AlliancePose2dSup(FieldConstants.getLineup(splitCommand)), swerveStateSup)); + commandList.add(s_Swerve.poseDriveCommand(new AlliancePose2dSup(FieldConstants.getLineup(splitCommand)), swerveStateSup)); commandList.add(Commands.waitSeconds(0.1)); commandList.add(s_Coral.setSpeedCommand(Constants.Coral.forwardSpeed).until(s_Coral::getSensor)); } case 'c' -> { - commandList.add(s_Swerve.poseLockDriveCommand(new AlliancePose2dSup(FieldConstants.getLineup(splitCommand)), swerveStateSup)); + commandList.add(s_Swerve.poseDriveCommand(new AlliancePose2dSup(FieldConstants.getLineup(splitCommand)), swerveStateSup)); commandList.add(Commands.waitUntil(s_Coral::getSensor)); } } diff --git a/src/main/java/frc/robot/util/FieldUtils.java b/src/main/java/frc/robot/util/FieldUtils.java index 1c36f7b..0d24187 100644 --- a/src/main/java/frc/robot/util/FieldUtils.java +++ b/src/main/java/frc/robot/util/FieldUtils.java @@ -3,8 +3,6 @@ import java.util.Arrays; import java.util.Optional; -import com.pathplanner.lib.path.PathPlannerPath; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -114,22 +112,13 @@ public static Line getNearestCoralStation(Translation2d robotPos) northHalf ? GeoFencing.cornerNBlue : GeoFencing.cornerSBlue; } - public static PathPlannerPath loadPath(String pathName) - { - try { - PathPlannerPath path = PathPlannerPath.fromPathFile(pathName); - return path; - } catch (Exception e) { - DriverStation.reportError(String.format("Unable to load path: %s", pathName), true); - } - return null; - } + public static boolean atReefLineUp(Pose2d robotPose) + {return Arrays.stream(FieldConstants.reefLineups).anyMatch(lineup -> atPose(robotPose, lineup));} - public static boolean atReefLineUp(Translation2d robotPos) + public static boolean atPose(Pose2d robotPose, Pose2d targetPose) { - return - Arrays.stream(FieldConstants.reefLineups) - .anyMatch(lineup -> Math.abs(lineup.getTranslation().minus(robotPos).getNorm()) < Constants.Control.lineupTolerance); + + return robotPose.getTranslation().getDistance(targetPose.getTranslation()) < Constants.Control.lineupTolerance && + Math.abs(robotPose.getRotation().getDegrees() - targetPose.getRotation().getDegrees()) < Constants.Control.angleLineupTolerance; } - } \ No newline at end of file diff --git a/vendordeps/PathplannerLib-2025.2.7.json b/vendordeps/PathplannerLib-2025.2.7.json deleted file mode 100644 index d3f84e5..0000000 --- a/vendordeps/PathplannerLib-2025.2.7.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "fileName": "PathplannerLib-2025.2.7.json", - "name": "PathplannerLib", - "version": "2025.2.7", - "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2025", - "mavenUrls": [ - "https://3015rangerrobotics.github.io/pathplannerlib/repo" - ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", - "javaDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-java", - "version": "2025.2.7" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-cpp", - "version": "2025.2.7", - "libName": "PathplannerLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxathena", - "linuxarm32", - "linuxarm64" - ] - } - ] -} \ No newline at end of file From 82b2ca948ef4d537095610cd7a9df44a8d36b14a Mon Sep 17 00:00:00 2001 From: ThePixelatedCat Date: Wed, 20 Aug 2025 15:02:19 +1000 Subject: [PATCH 10/13] Mostly cleaning up Superstructure (organisation, removing unneeded initialisation, anonymising methods) --- src/main/java/frc/robot/Superstructure.java | 209 ++++++------------ .../frc/robot/subsystems/vision/Vision.java | 64 +++--- src/main/java/frc/robot/util/FieldUtils.java | 17 +- .../JoystickTransmuter.java | 2 +- 4 files changed, 106 insertions(+), 186 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 91859ac..9a9b07d 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -1,12 +1,7 @@ package frc.robot; -import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.XboxController.Axis; import edu.wpi.first.wpilibj.smartdashboard.Field2d; @@ -16,11 +11,12 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; import frc.robot.commands.swerve.*; import frc.robot.constants.*; +import static frc.robot.constants.FieldConstants.*; +import static frc.robot.constants.FieldConstants.GeoFencing.*; import frc.robot.subsystems.*; import frc.robot.subsystems.vision.*; import frc.robot.subsystems.vision.Vision.TagPOI; @@ -35,30 +31,41 @@ public class Superstructure public enum TargetPosition {Left, Right, Centre, None} public enum DriveState {Reef, Station, Barge, None} - private static boolean rotationKnown = false; - private static boolean useLimelights = true; - private static boolean useFence = true; - private boolean redAlliance; - + /* State */ private SwerveDriveState swerveState; - private Field2d field; - - private final Telemetry ctreLogger; - - private final CommandSwerveDrivetrain s_Swerve; - private final CoralRoller s_Coral; - private final Vision s_Vision; - private TargetPosition currentTarget; private DriveState currentDriveState; + /* Telemetry and SD */ + private Field2d field = new Field2d(); + private final Telemetry ctreLogger = new Telemetry(Constants.Swerve.maxSpeed); + + /* Subsystems */ + private final CommandSwerveDrivetrain s_Swerve = TunerConstants.createDrivetrain(); + private final CoralRoller s_Coral = new CoralRoller(); + private final Vision s_Vision = new Vision + ( + (poseEst, timestmp, stdDevs) -> + { + s_Swerve.setVisionMeasurementStdDevs(stdDevs); + s_Swerve.addVisionMeasurement(poseEst, timestmp); + }, + () -> Pair.of(s_Swerve.getPigeon2().getYaw().getValueAsDouble(), swerveState.Speeds.omegaRadiansPerSecond), + new Limelight("limelight-fore"), + new Limelight("limelight-aft") + ); + + /* Controllers */ private final CommandXboxController driver = new CommandXboxController(0); private final CommandXboxController operator = new CommandXboxController(1); + + /* Rumble */ private final RumbleRequester io_driverRight = new RumbleRequester(driver, RumbleType.kRightRumble, SD.RUMBLE_DRIVER); private final RumbleRequester io_driverLeft = new RumbleRequester(driver, RumbleType.kLeftRumble, SD.RUMBLE_DRIVER); private final RumbleRequester io_operatorRight = new RumbleRequester(operator, RumbleType.kRightRumble, SD.RUMBLE_OPERATOR); private final RumbleRequester io_operatorLeft = new RumbleRequester(operator, RumbleType.kLeftRumble, SD.RUMBLE_OPERATOR); + /* Input Transmutation */ private final JoystickTransmuter driverStick = new JoystickTransmuter(driver::getLeftY, driver::getLeftX).invertX().invertY(); private final Brake driverBrake = new Brake(driver::getRightTriggerAxis, Constants.Control.maxThrottle, Constants.Control.minThrottle); private final InputCurve driverInputCurve = new InputCurve(2); @@ -66,51 +73,43 @@ public enum DriveState {Reef, Station, Barge, None} public Superstructure() { - field = new Field2d(); - s_Swerve = TunerConstants.createDrivetrain(); - s_Coral = new CoralRoller(); - s_Vision = new Vision(this::applyVisionEstimate, () -> Pair.of(getYaw(), swerveState.Speeds.omegaRadiansPerSecond), new Limelight("limelight-fore"), new Limelight("limelight-aft")); - - ctreLogger = new Telemetry(Constants.Swerve.maxSpeed); - - redAlliance = FieldUtils.isRedAlliance(); - - setStartPose(redAlliance); + /* State Initialisation */ + currentDriveState = DriveState.None; updateSwerveState(); - driverStick.rotated(redAlliance); - FieldUtils.activateAllianceFencing(); - + + /* Telemetry and SD */ SmartDashboard.putData("Field", field); - s_Swerve.registerTelemetry(ctreLogger::telemeterize); - driverStick.withFieldObjects(FieldConstants.GeoFencing.fieldGeoFence).withBrake(driverBrake).withInputCurve(driverInputCurve).withDeadband(driverDeadband); - FieldConstants.GeoFencing.fieldGeoFence.setActiveCondition(() -> useFence && useLimelights); + + /* Configure Input Transmutation */ + boolean redAlliance = FieldUtils.isRedAlliance(); + driverStick.rotated(redAlliance); + driverStick.withFieldObjects(GeoFencing.fieldGeoFence).withBrake(driverBrake).withInputCurve(driverInputCurve).withDeadband(driverDeadband); + FieldUtils.activateAllianceFencing(redAlliance); FieldConstants.GeoFencing.configureAttractors((testTarget, testState) -> currentTarget == testTarget && currentDriveState == testState); - FieldObject.setRobotRadiusSup(this::robotRadiusSup); - FieldObject.setRobotPosSup(this::getPosition); + FieldObject.setRobotRadiusSup + (() -> + Math.hypot(swerveState.Speeds.vxMetersPerSecond, swerveState.Speeds.vyMetersPerSecond) >= robotSpeedThreshold ? + robotRadiusCircumscribed : + robotRadiusInscribed + ); + FieldObject.setRobotPosSup(swerveState.Pose::getTranslation); + /* Bindings */ bindControls(); bindRumbles(); bindSD(); } - public SwerveDriveState getSwerveState() - {return swerveState;} - - public void updateSwerveState() + private void updateSwerveState() { swerveState = s_Swerve.getState(); field.setRobotPose(swerveState.Pose); } - public Translation2d getPosition() - { - return swerveState.Pose.getTranslation(); - } - private void bindControls() { - currentDriveState = DriveState.None; + /* Default Commands */ s_Swerve.setDefaultCommand ( new ManualDrive @@ -121,9 +120,26 @@ private void bindControls() driver::getRightTriggerAxis ) ); - s_Coral.setDefaultCommand(s_Coral.setSpeedCommand(0)); + /* Setting Drive States */ + driver.povLeft().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.Left)); + driver.povRight().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.Right)); + driver.povUp().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.Centre)); + driver.povDown().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.None)); + + driver.x().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.Reef)); + driver.a().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.Station)); + driver.y().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.Barge)); + driver.b().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.None)); + driver.axisMagnitudeGreaterThan(Axis.kRightX.value, 0.2).onTrue(Commands.runOnce(() -> currentDriveState = DriveState.None)); + + /* Coral Roller */ + driver.leftTrigger().whileTrue(s_Coral.setSpeedCommand(Constants.Coral.forwardSpeed)); + driver.leftBumper().whileTrue(s_Coral.setSpeedCommand(Constants.Coral.reverseSpeed)); + new Trigger(() -> FieldUtils.atReefLineUp(swerveState.Pose)).whileTrue(s_Coral.setSpeedCommand(Constants.Coral.forwardSpeed)); + + /* Heading Locking */ new Trigger(() -> currentDriveState == DriveState.None) .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.REEF))) .whileTrue @@ -136,7 +152,6 @@ private void bindControls() driver::getRightTriggerAxis ) ); - new Trigger(() -> currentDriveState == DriveState.Reef) .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.REEF))) .whileTrue @@ -146,10 +161,9 @@ private void bindControls() s_Swerve, driverStick::stickOutput, Rotation2d.kZero, - () -> getPosition() + swerveState.Pose::getTranslation ) ); - new Trigger(() -> currentDriveState == DriveState.Station) .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.CORALSTATION))) .whileTrue @@ -159,10 +173,9 @@ private void bindControls() s_Swerve, driverStick::stickOutput, Rotation2d.kZero, - () -> getPosition() + swerveState.Pose::getTranslation ) ); - new Trigger(() -> currentDriveState == DriveState.Barge) .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.BARGE))) .whileTrue @@ -173,66 +186,12 @@ private void bindControls() driverStick::stickOutput, Rotation2d.kZero, Rotation2d.kZero, - () -> getPosition() - ) - ); - - - driver.leftTrigger().whileTrue(s_Coral.setSpeedCommand(Constants.Coral.forwardSpeed)); - driver.leftBumper().whileTrue(s_Coral.setSpeedCommand(Constants.Coral.reverseSpeed)); - //operator.leftBumper().whileTrue(s_Coral.runCommand(SD.IO_CORALSPEED_R.get())); - // Heading reset - driver.start() - .onTrue - ( - Commands.runOnce - ( - () -> - { - Pigeon2 pigeon = s_Swerve.getPigeon2(); - - pigeon.setYaw(redAlliance ? 0 : 180); - s_Swerve.resetPose(new Pose2d(swerveState.Pose.getTranslation(), new Rotation2d(Math.toRadians(pigeon.getYaw().getValueAsDouble())))); - FieldUtils.activateAllianceFencing(); - rotationKnown = false; - useLimelights = true; - } - ) - .ignoringDisable(true) - .withName("HeadingReset") - ); - driver.back() - .onTrue - ( - Commands.runOnce - ( - () -> - { - Pigeon2 pigeon = s_Swerve.getPigeon2(); - - pigeon.setYaw(redAlliance ? 0 : 180); - s_Swerve.resetPose(new Pose2d(swerveState.Pose.getTranslation(), new Rotation2d(Math.toRadians(pigeon.getYaw().getValueAsDouble())))); - FieldUtils.activateAllianceFencing(); - rotationKnown = false; - useLimelights = false; - } + swerveState.Pose::getTranslation ) - .ignoringDisable(true) - .withName("DisableNavigation") ); - driver.povLeft().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.Left)); - driver.povRight().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.Right)); - driver.povUp().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.Centre)); - driver.povDown().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.None)); - - driver.x().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.Reef)); - driver.a().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.Station)); - driver.y().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.Barge)); - driver.b().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.None)); - driver.axisMagnitudeGreaterThan(Axis.kRightX.value, 0.2).onTrue(Commands.runOnce(() -> currentDriveState = DriveState.None)); - - new Trigger(() -> FieldUtils.atReefLineUp(swerveState.Pose)).whileTrue(s_Coral.setSpeedCommand(Constants.Coral.forwardSpeed)); + /* Other */ + driver.start().onTrue(Commands.runOnce(s_Vision::resetRotation).ignoringDisable(true)); } private void bindRumbles() @@ -247,12 +206,6 @@ private void bindSD() new Trigger(SD.LL_EXPOSURE_DOWN::button).onTrue(Commands.runOnce(s_Vision::decrementPipeline)); } - private void applyVisionEstimate(Pose2d poseEstMeters, double timestampSeconds, Matrix stdDevs) - { - s_Swerve.setVisionMeasurementStdDevs(stdDevs); - s_Swerve.addVisionMeasurement(poseEstMeters, timestampSeconds); - } - public void periodic() { updateSwerveState(); @@ -262,28 +215,6 @@ public Command getAutonomousCommand() { return AutoFactories.getCommandList(SD.AUTO_STRING.get(), s_Coral, s_Swerve, () -> swerveState); } - - public double robotRadiusSup() - { - double robotSpeed = Math.hypot(swerveState.Speeds.vxMetersPerSecond, swerveState.Speeds.vyMetersPerSecond); - - return - robotSpeed >= FieldConstants.GeoFencing.robotSpeedThreshold ? - FieldConstants.GeoFencing.robotRadiusCircumscribed : - FieldConstants.GeoFencing.robotRadiusInscribed; - } - - private void setStartPose(boolean isRedAlliance) - { - if (isRedAlliance) - {s_Swerve.resetPose(FieldConstants.redStartLine);} - else - {s_Swerve.resetPose(FieldConstants.blueStartLine);} - } - public static boolean isRotationKnown() {return rotationKnown;} - public static void setRotationKnown(boolean isKnown) {rotationKnown = isKnown;} - public static boolean isVisionActive() {return useLimelights;} - public void setYaw(double newYaw) {s_Swerve.getPigeon2().setYaw(newYaw);} - public double getYaw() {return s_Swerve.getPigeon2().getYaw().getValueAsDouble();} + public void setYaw(double newYaw) {s_Swerve.getPigeon2().setYaw(newYaw);} } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 1e23a7e..71b5fe1 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -77,6 +77,8 @@ public void decrementPipeline() SD.LL_EXPOSURE.put((double)pipelineIndex); } + public void resetRotation() {rotationKnown = false;} + @Override public void periodic() { @@ -104,47 +106,43 @@ public void periodic() } } - if (Superstructure.isVisionActive()) + if (!rotationKnown) { - rotationKnown = Superstructure.isRotationKnown(); + lastCycleRotationKnown = false; - if (!rotationKnown) + for (var ll : lls) { - lastCycleRotationKnown = false; - - for (var ll : lls) - { - ll.getLimelightRotation().ifPresent - ( - rotationReading -> - { - rotationBuf.add(0, rotationReading.getDegrees()); + ll.getLimelightRotation().ifPresent + ( + rotationReading -> + { + rotationBuf.add(0, rotationReading.getDegrees()); + + if (rotationBuf.size() > mt1CyclesNeeded) + {rotationBuf.remove(mt1CyclesNeeded);} - if (rotationBuf.size() > mt1CyclesNeeded) - {rotationBuf.remove(mt1CyclesNeeded);} - - if (rotationBuf.size() == mt1CyclesNeeded) + if (rotationBuf.size() == mt1CyclesNeeded) + { + double lowest = rotationBuf.get(0).doubleValue(); + double highest = rotationBuf.get(0).doubleValue(); + + for(var reading : rotationBuf) + { + lowest = Math.min(lowest, reading.doubleValue()); + highest = Math.max(highest, reading.doubleValue()); + } + + if (highest - lowest < 1) { - double lowest = rotationBuf.get(0).doubleValue(); - double highest = rotationBuf.get(0).doubleValue(); - - for(var reading : rotationBuf) - { - lowest = Math.min(lowest, reading.doubleValue()); - highest = Math.max(highest, reading.doubleValue()); - } - - if (highest - lowest < 1) - { - rotationKnown = true; - Superstructure.setRotationKnown(true); - Superstructure.setYaw((highest + lowest) / 2); - } + rotationKnown = true; + Superstructure.setRotationKnown(true); + Superstructure.setYaw((highest + lowest) / 2); } } - ); - } + } + ); } + if (!lastCycleRotationKnown) { diff --git a/src/main/java/frc/robot/util/FieldUtils.java b/src/main/java/frc/robot/util/FieldUtils.java index 0d24187..0cf2fd9 100644 --- a/src/main/java/frc/robot/util/FieldUtils.java +++ b/src/main/java/frc/robot/util/FieldUtils.java @@ -1,7 +1,6 @@ package frc.robot.util; import java.util.ArrayList; import java.util.Arrays; -import java.util.Optional; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -17,7 +16,7 @@ public class FieldUtils { public static boolean isRedAlliance() { - Optional alliance = DriverStation.getAlliance(); + var alliance = DriverStation.getAlliance(); return alliance.isPresent() && alliance.get() == Alliance.Red; } @@ -60,18 +59,10 @@ public static Pose2d rotatePose(Pose2d pose) return pose; } - public static void activateAllianceFencing() + public static void activateAllianceFencing(boolean redAlliance) { - if (isRedAlliance()) - { - GeoFencing.fieldRedGeoFence.setActiveCondition(() -> true); - GeoFencing.fieldBlueGeoFence.setActiveCondition(() -> false); - } - else - { - GeoFencing.fieldBlueGeoFence.setActiveCondition(() -> true); - GeoFencing.fieldRedGeoFence.setActiveCondition(() -> false); - } + GeoFencing.fieldRedGeoFence.setActiveCondition(() -> redAlliance); + GeoFencing.fieldBlueGeoFence.setActiveCondition(() -> !redAlliance); } public static int getNearestReefFaceAllianceLocked(Translation2d robotPos) diff --git a/src/main/java/frc/robot/util/controlTransmutation/JoystickTransmuter.java b/src/main/java/frc/robot/util/controlTransmutation/JoystickTransmuter.java index 3335598..aa7c89b 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/JoystickTransmuter.java +++ b/src/main/java/frc/robot/util/controlTransmutation/JoystickTransmuter.java @@ -41,7 +41,7 @@ public JoystickTransmuter(DoubleSupplier inputX, DoubleSupplier inputY) brake = new Brake(); fieldObjectList = new ObjectList(); - stickOutputSup = () -> stickOutput(); + stickOutputSup = this::stickOutput; } /** From e917c0377d3575eb7d3d0897af9fa1badf2fad3b Mon Sep 17 00:00:00 2001 From: ThePixelatedCat Date: Wed, 20 Aug 2025 15:50:43 +1000 Subject: [PATCH 11/13] Combined Superstructure into Robot --- src/main/java/frc/robot/Robot.java | 215 ++++++++++++++++- src/main/java/frc/robot/Superstructure.java | 220 ------------------ .../frc/robot/constants/FieldConstants.java | 4 +- .../frc/robot/subsystems/vision/Vision.java | 6 +- 4 files changed, 210 insertions(+), 235 deletions(-) delete mode 100644 src/main/java/frc/robot/Superstructure.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cba39a6..298753e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,52 +4,249 @@ package frc.robot; -import com.ctre.phoenix6.SignalLogger; - import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj.XboxController.Axis; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; + +import frc.robot.commands.swerve.*; +import frc.robot.constants.*; +import static frc.robot.constants.FieldConstants.*; +import static frc.robot.constants.FieldConstants.GeoFencing.*; +import frc.robot.subsystems.*; +import frc.robot.subsystems.vision.*; +import frc.robot.subsystems.vision.Vision.TagPOI; +import frc.robot.util.AutoFactories; +import frc.robot.util.FieldUtils; +import frc.robot.util.SD; +import frc.robot.util.controlTransmutation.*; +import frc.robot.util.libs.Telemetry; + @Logged public class Robot extends TimedRobot { - private Command m_autonomousCommand; + public enum TargetPosition {Left, Right, Centre, None} + public enum DriveState {Reef, Station, Barge, None} + + + /* State */ + private SwerveDriveState swerveState; + private TargetPosition currentTarget; + private DriveState currentDriveState; + private Command autoCommand; + + /* Telemetry and SD */ + private Field2d field = new Field2d(); + private final Telemetry ctreLogger = new Telemetry(Constants.Swerve.maxSpeed); + + /* Subsystems */ + private final CommandSwerveDrivetrain s_Swerve = TunerConstants.createDrivetrain(); + private final CoralRoller s_Coral = new CoralRoller(); + private final Vision s_Vision = new Vision + ( + (poseEst, timestmp, stdDevs) -> + { + s_Swerve.setVisionMeasurementStdDevs(stdDevs); + s_Swerve.addVisionMeasurement(poseEst, timestmp); + }, + () -> Pair.of(s_Swerve.getPigeon2().getYaw().getValueAsDouble(), swerveState.Speeds.omegaRadiansPerSecond), + new Limelight("limelight-fore"), + new Limelight("limelight-aft") + ); + + /* Controllers */ + private final CommandXboxController driver = new CommandXboxController(0); + private final CommandXboxController operator = new CommandXboxController(1); - private final Superstructure superstructure; + /* Rumble */ + private final RumbleRequester io_driverRight = new RumbleRequester(driver, RumbleType.kRightRumble, SD.RUMBLE_DRIVER); + private final RumbleRequester io_driverLeft = new RumbleRequester(driver, RumbleType.kLeftRumble, SD.RUMBLE_DRIVER); + private final RumbleRequester io_operatorRight = new RumbleRequester(operator, RumbleType.kRightRumble, SD.RUMBLE_OPERATOR); + private final RumbleRequester io_operatorLeft = new RumbleRequester(operator, RumbleType.kLeftRumble, SD.RUMBLE_OPERATOR); + + /* Input Transmutation */ + private final JoystickTransmuter driverStick = new JoystickTransmuter(driver::getLeftY, driver::getLeftX).invertX().invertY(); + private final Brake driverBrake = new Brake(driver::getRightTriggerAxis, Constants.Control.maxThrottle, Constants.Control.minThrottle); + private final InputCurve driverInputCurve = new InputCurve(2); + private final Deadband driverDeadband = new Deadband(); public Robot() { - superstructure = new Superstructure(); + /* State Initialisation */ + currentDriveState = DriveState.None; + updateSwerveState(); + /* Telemetry and SD */ SignalLogger.enableAutoLogging(false); DataLogManager.start("/home/lvuser/logs"); DriverStation.startDataLog(DataLogManager.getLog()); Epilogue.bind(this); + SmartDashboard.putData("Field", field); + s_Swerve.registerTelemetry(ctreLogger::telemeterize); + + /* Configure Input Transmutation */ + boolean redAlliance = FieldUtils.isRedAlliance(); + driverStick.rotated(redAlliance); + driverStick.withFieldObjects(GeoFencing.fieldGeoFence).withBrake(driverBrake).withInputCurve(driverInputCurve).withDeadband(driverDeadband); + FieldUtils.activateAllianceFencing(redAlliance); + FieldConstants.GeoFencing.configureAttractors((testTarget, testState) -> currentTarget == testTarget && currentDriveState == testState); + FieldObject.setRobotRadiusSup + (() -> + Math.hypot(swerveState.Speeds.vxMetersPerSecond, swerveState.Speeds.vyMetersPerSecond) >= robotSpeedThreshold ? + robotRadiusCircumscribed : + robotRadiusInscribed + ); + FieldObject.setRobotPosSup(swerveState.Pose::getTranslation); + + /* Bindings */ + bindControls(); + bindRumbles(); + bindSD(); + } + private void updateSwerveState() + { + swerveState = s_Swerve.getState(); + field.setRobotPose(swerveState.Pose); + } + + private void bindControls() + { + /* Default Commands */ + s_Swerve.setDefaultCommand + ( + new ManualDrive + ( + s_Swerve, + driverStick::stickOutput, + () -> -driver.getRightX(), + driver::getRightTriggerAxis + ) + ); + s_Coral.setDefaultCommand(s_Coral.setSpeedCommand(0)); + + /* Setting Drive States */ + driver.povLeft().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.Left)); + driver.povRight().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.Right)); + driver.povUp().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.Centre)); + driver.povDown().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.None)); + + driver.x().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.Reef)); + driver.a().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.Station)); + driver.y().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.Barge)); + driver.b().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.None)); + driver.axisMagnitudeGreaterThan(Axis.kRightX.value, 0.2).onTrue(Commands.runOnce(() -> currentDriveState = DriveState.None)); + + /* Coral Roller */ + driver.leftTrigger().whileTrue(s_Coral.setSpeedCommand(Constants.Coral.forwardSpeed)); + driver.leftBumper().whileTrue(s_Coral.setSpeedCommand(Constants.Coral.reverseSpeed)); + new Trigger(() -> FieldUtils.atReefLineUp(swerveState.Pose)).whileTrue(s_Coral.setSpeedCommand(Constants.Coral.forwardSpeed)); + + /* Heading Locking */ + new Trigger(() -> currentDriveState == DriveState.None) + .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.REEF))) + .whileTrue + ( + new ManualDrive + ( + s_Swerve, + driverStick::stickOutput, + () -> -driver.getRightX(), + driver::getRightTriggerAxis + ) + ); + new Trigger(() -> currentDriveState == DriveState.Reef) + .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.REEF))) + .whileTrue + ( + new TargetScoreDrive + ( + s_Swerve, + driverStick::stickOutput, + Rotation2d.kZero, + swerveState.Pose::getTranslation + ) + ); + new Trigger(() -> currentDriveState == DriveState.Station) + .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.CORALSTATION))) + .whileTrue + ( + new TargetStationDrive + ( + s_Swerve, + driverStick::stickOutput, + Rotation2d.kZero, + swerveState.Pose::getTranslation + ) + ); + new Trigger(() -> currentDriveState == DriveState.Barge) + .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.BARGE))) + .whileTrue + ( + new HeadingLockedDrive + ( + s_Swerve, + driverStick::stickOutput, + Rotation2d.kZero, + Rotation2d.kZero, + swerveState.Pose::getTranslation + ) + ); + + /* Other */ + driver.start().onTrue(Commands.runOnce(s_Vision::resetRotation).ignoringDisable(true)); + } + + private void bindRumbles() + { + io_operatorLeft.addRumbleTrigger("CoralHeld", new Trigger(s_Coral::getSensor)); + io_operatorRight.addRumbleTrigger("ScoreReady" , new Trigger(() -> FieldUtils.atReefLineUp(swerveState.Pose))); + } + + private void bindSD() + { + new Trigger(SD.LL_EXPOSURE_UP::button).onTrue(Commands.runOnce(s_Vision::incrementPipeline)); + new Trigger(SD.LL_EXPOSURE_DOWN::button).onTrue(Commands.runOnce(s_Vision::decrementPipeline)); + } + + public void setYaw(double newYaw) {s_Swerve.getPigeon2().setYaw(newYaw);} + @Override public void robotPeriodic() { - superstructure.periodic(); + updateSwerveState(); CommandScheduler.getInstance().run(); } @Override public void autonomousInit() { - m_autonomousCommand = superstructure.getAutonomousCommand(); + autoCommand = AutoFactories.getCommandList(SD.AUTO_STRING.get(), s_Coral, s_Swerve, () -> swerveState); - if (m_autonomousCommand != null) m_autonomousCommand.schedule(); + if (autoCommand != null) autoCommand.schedule(); } @Override public void teleopInit() { - if (m_autonomousCommand != null) m_autonomousCommand.cancel(); + if (autoCommand != null) autoCommand.cancel(); } @Override diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java deleted file mode 100644 index 9a9b07d..0000000 --- a/src/main/java/frc/robot/Superstructure.java +++ /dev/null @@ -1,220 +0,0 @@ -package frc.robot; - -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.GenericHID.RumbleType; -import edu.wpi.first.wpilibj.XboxController.Axis; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; - -import frc.robot.commands.swerve.*; -import frc.robot.constants.*; -import static frc.robot.constants.FieldConstants.*; -import static frc.robot.constants.FieldConstants.GeoFencing.*; -import frc.robot.subsystems.*; -import frc.robot.subsystems.vision.*; -import frc.robot.subsystems.vision.Vision.TagPOI; -import frc.robot.util.AutoFactories; -import frc.robot.util.FieldUtils; -import frc.robot.util.SD; -import frc.robot.util.controlTransmutation.*; -import frc.robot.util.libs.Telemetry; - -public class Superstructure -{ - public enum TargetPosition {Left, Right, Centre, None} - public enum DriveState {Reef, Station, Barge, None} - - /* State */ - private SwerveDriveState swerveState; - private TargetPosition currentTarget; - private DriveState currentDriveState; - - /* Telemetry and SD */ - private Field2d field = new Field2d(); - private final Telemetry ctreLogger = new Telemetry(Constants.Swerve.maxSpeed); - - /* Subsystems */ - private final CommandSwerveDrivetrain s_Swerve = TunerConstants.createDrivetrain(); - private final CoralRoller s_Coral = new CoralRoller(); - private final Vision s_Vision = new Vision - ( - (poseEst, timestmp, stdDevs) -> - { - s_Swerve.setVisionMeasurementStdDevs(stdDevs); - s_Swerve.addVisionMeasurement(poseEst, timestmp); - }, - () -> Pair.of(s_Swerve.getPigeon2().getYaw().getValueAsDouble(), swerveState.Speeds.omegaRadiansPerSecond), - new Limelight("limelight-fore"), - new Limelight("limelight-aft") - ); - - /* Controllers */ - private final CommandXboxController driver = new CommandXboxController(0); - private final CommandXboxController operator = new CommandXboxController(1); - - /* Rumble */ - private final RumbleRequester io_driverRight = new RumbleRequester(driver, RumbleType.kRightRumble, SD.RUMBLE_DRIVER); - private final RumbleRequester io_driverLeft = new RumbleRequester(driver, RumbleType.kLeftRumble, SD.RUMBLE_DRIVER); - private final RumbleRequester io_operatorRight = new RumbleRequester(operator, RumbleType.kRightRumble, SD.RUMBLE_OPERATOR); - private final RumbleRequester io_operatorLeft = new RumbleRequester(operator, RumbleType.kLeftRumble, SD.RUMBLE_OPERATOR); - - /* Input Transmutation */ - private final JoystickTransmuter driverStick = new JoystickTransmuter(driver::getLeftY, driver::getLeftX).invertX().invertY(); - private final Brake driverBrake = new Brake(driver::getRightTriggerAxis, Constants.Control.maxThrottle, Constants.Control.minThrottle); - private final InputCurve driverInputCurve = new InputCurve(2); - private final Deadband driverDeadband = new Deadband(); - - public Superstructure() - { - /* State Initialisation */ - currentDriveState = DriveState.None; - updateSwerveState(); - - /* Telemetry and SD */ - SmartDashboard.putData("Field", field); - s_Swerve.registerTelemetry(ctreLogger::telemeterize); - - /* Configure Input Transmutation */ - boolean redAlliance = FieldUtils.isRedAlliance(); - driverStick.rotated(redAlliance); - driverStick.withFieldObjects(GeoFencing.fieldGeoFence).withBrake(driverBrake).withInputCurve(driverInputCurve).withDeadband(driverDeadband); - FieldUtils.activateAllianceFencing(redAlliance); - FieldConstants.GeoFencing.configureAttractors((testTarget, testState) -> currentTarget == testTarget && currentDriveState == testState); - FieldObject.setRobotRadiusSup - (() -> - Math.hypot(swerveState.Speeds.vxMetersPerSecond, swerveState.Speeds.vyMetersPerSecond) >= robotSpeedThreshold ? - robotRadiusCircumscribed : - robotRadiusInscribed - ); - FieldObject.setRobotPosSup(swerveState.Pose::getTranslation); - - /* Bindings */ - bindControls(); - bindRumbles(); - bindSD(); - } - - private void updateSwerveState() - { - swerveState = s_Swerve.getState(); - field.setRobotPose(swerveState.Pose); - } - - private void bindControls() - { - /* Default Commands */ - s_Swerve.setDefaultCommand - ( - new ManualDrive - ( - s_Swerve, - driverStick::stickOutput, - () -> -driver.getRightX(), - driver::getRightTriggerAxis - ) - ); - s_Coral.setDefaultCommand(s_Coral.setSpeedCommand(0)); - - /* Setting Drive States */ - driver.povLeft().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.Left)); - driver.povRight().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.Right)); - driver.povUp().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.Centre)); - driver.povDown().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.None)); - - driver.x().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.Reef)); - driver.a().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.Station)); - driver.y().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.Barge)); - driver.b().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.None)); - driver.axisMagnitudeGreaterThan(Axis.kRightX.value, 0.2).onTrue(Commands.runOnce(() -> currentDriveState = DriveState.None)); - - /* Coral Roller */ - driver.leftTrigger().whileTrue(s_Coral.setSpeedCommand(Constants.Coral.forwardSpeed)); - driver.leftBumper().whileTrue(s_Coral.setSpeedCommand(Constants.Coral.reverseSpeed)); - new Trigger(() -> FieldUtils.atReefLineUp(swerveState.Pose)).whileTrue(s_Coral.setSpeedCommand(Constants.Coral.forwardSpeed)); - - /* Heading Locking */ - new Trigger(() -> currentDriveState == DriveState.None) - .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.REEF))) - .whileTrue - ( - new ManualDrive - ( - s_Swerve, - driverStick::stickOutput, - () -> -driver.getRightX(), - driver::getRightTriggerAxis - ) - ); - new Trigger(() -> currentDriveState == DriveState.Reef) - .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.REEF))) - .whileTrue - ( - new TargetScoreDrive - ( - s_Swerve, - driverStick::stickOutput, - Rotation2d.kZero, - swerveState.Pose::getTranslation - ) - ); - new Trigger(() -> currentDriveState == DriveState.Station) - .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.CORALSTATION))) - .whileTrue - ( - new TargetStationDrive - ( - s_Swerve, - driverStick::stickOutput, - Rotation2d.kZero, - swerveState.Pose::getTranslation - ) - ); - new Trigger(() -> currentDriveState == DriveState.Barge) - .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.BARGE))) - .whileTrue - ( - new HeadingLockedDrive - ( - s_Swerve, - driverStick::stickOutput, - Rotation2d.kZero, - Rotation2d.kZero, - swerveState.Pose::getTranslation - ) - ); - - /* Other */ - driver.start().onTrue(Commands.runOnce(s_Vision::resetRotation).ignoringDisable(true)); - } - - private void bindRumbles() - { - io_operatorLeft.addRumbleTrigger("CoralHeld", new Trigger(s_Coral::getSensor)); - io_operatorRight.addRumbleTrigger("ScoreReady" , new Trigger(() -> FieldUtils.atReefLineUp(swerveState.Pose))); - } - - private void bindSD() - { - new Trigger(SD.LL_EXPOSURE_UP::button).onTrue(Commands.runOnce(s_Vision::incrementPipeline)); - new Trigger(SD.LL_EXPOSURE_DOWN::button).onTrue(Commands.runOnce(s_Vision::decrementPipeline)); - } - - public void periodic() - { - updateSwerveState(); - } - - public Command getAutonomousCommand() - { - return AutoFactories.getCommandList(SD.AUTO_STRING.get(), s_Coral, s_Swerve, () -> swerveState); - } - - public void setYaw(double newYaw) {s_Swerve.getPigeon2().setYaw(newYaw);} -} diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java index 95bae21..cf79341 100644 --- a/src/main/java/frc/robot/constants/FieldConstants.java +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -7,8 +7,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; -import frc.robot.Superstructure.DriveState; -import frc.robot.Superstructure.TargetPosition; +import frc.robot.Robot.DriveState; +import frc.robot.Robot.TargetPosition; import frc.robot.util.controlTransmutation.Attractor; import frc.robot.util.controlTransmutation.ObjectList; import frc.robot.util.controlTransmutation.geoFence.*; diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 71b5fe1..77ed3a0 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -22,7 +22,7 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Superstructure; +import frc.robot.Robot; import frc.robot.util.SD; import frc.robot.util.libs.LimelightHelpers; import static frc.robot.constants.Constants.Vision.*; @@ -135,14 +135,12 @@ public void periodic() if (highest - lowest < 1) { rotationKnown = true; - Superstructure.setRotationKnown(true); - Superstructure.setYaw((highest + lowest) / 2); + Robot.setYaw((highest + lowest) / 2); } } } ); } - if (!lastCycleRotationKnown) { From 96d634e9c6d5ac69c3e9b322fba1c33a7a3efbd5 Mon Sep 17 00:00:00 2001 From: ThePixelatedCat Date: Wed, 20 Aug 2025 16:05:04 +1000 Subject: [PATCH 12/13] Got vision working. End of refactor, remaining changes will be in a new repo for long-term codebase --- src/main/java/frc/robot/Robot.java | 30 ++++++++----------- .../frc/robot/subsystems/vision/Vision.java | 5 ---- 2 files changed, 13 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 298753e..e70fb50 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -37,14 +37,13 @@ import frc.robot.util.controlTransmutation.*; import frc.robot.util.libs.Telemetry; - @Logged public class Robot extends TimedRobot { + /* Enums */ public enum TargetPosition {Left, Right, Centre, None} public enum DriveState {Reef, Station, Barge, None} - /* State */ private SwerveDriveState swerveState; private TargetPosition currentTarget; @@ -56,7 +55,7 @@ public enum DriveState {Reef, Station, Barge, None} private final Telemetry ctreLogger = new Telemetry(Constants.Swerve.maxSpeed); /* Subsystems */ - private final CommandSwerveDrivetrain s_Swerve = TunerConstants.createDrivetrain(); + private final static CommandSwerveDrivetrain s_Swerve = TunerConstants.createDrivetrain(); private final CoralRoller s_Coral = new CoralRoller(); private final Vision s_Vision = new Vision ( @@ -117,16 +116,9 @@ public Robot() /* Bindings */ bindControls(); bindRumbles(); - bindSD(); - - } - - private void updateSwerveState() - { - swerveState = s_Swerve.getState(); - field.setRobotPose(swerveState.Pose); } + /* Binding Methods */ private void bindControls() { /* Default Commands */ @@ -212,6 +204,8 @@ private void bindControls() /* Other */ driver.start().onTrue(Commands.runOnce(s_Vision::resetRotation).ignoringDisable(true)); + new Trigger(SD.LL_EXPOSURE_UP::button).onTrue(Commands.runOnce(s_Vision::incrementPipeline)); + new Trigger(SD.LL_EXPOSURE_DOWN::button).onTrue(Commands.runOnce(s_Vision::decrementPipeline)); } private void bindRumbles() @@ -220,14 +214,16 @@ private void bindRumbles() io_operatorRight.addRumbleTrigger("ScoreReady" , new Trigger(() -> FieldUtils.atReefLineUp(swerveState.Pose))); } - private void bindSD() + /* Util Methods */ + public static void setYaw(double newYaw) {s_Swerve.getPigeon2().setYaw(newYaw);} + + private void updateSwerveState() { - new Trigger(SD.LL_EXPOSURE_UP::button).onTrue(Commands.runOnce(s_Vision::incrementPipeline)); - new Trigger(SD.LL_EXPOSURE_DOWN::button).onTrue(Commands.runOnce(s_Vision::decrementPipeline)); + swerveState = s_Swerve.getState(); + field.setRobotPose(swerveState.Pose); } - - public void setYaw(double newYaw) {s_Swerve.getPigeon2().setYaw(newYaw);} - + + /* Opmode Methods */ @Override public void robotPeriodic() { diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 77ed3a0..ea73a2f 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -5,7 +5,6 @@ package frc.robot.subsystems.vision; import java.util.ArrayList; -import java.util.Optional; import java.util.function.Supplier; import com.ctre.phoenix6.Utils; @@ -15,16 +14,12 @@ import edu.wpi.first.math.Pair; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.util.SD; -import frc.robot.util.libs.LimelightHelpers; import static frc.robot.constants.Constants.Vision.*; public class Vision extends SubsystemBase From c3979bdd17446d9856ced54661a8c9f5e6f1ca78 Mon Sep 17 00:00:00 2001 From: ThePixelatedCat Date: Sat, 27 Sep 2025 10:22:38 +1000 Subject: [PATCH 13/13] Moved limelight names to IDConstants --- src/main/java/frc/robot/Robot.java | 5 +++-- src/main/java/frc/robot/constants/IDConstants.java | 9 +++++++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e70fb50..99caf6d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -26,6 +26,7 @@ import frc.robot.commands.swerve.*; import frc.robot.constants.*; +import static frc.robot.constants.IDConstants.*; import static frc.robot.constants.FieldConstants.*; import static frc.robot.constants.FieldConstants.GeoFencing.*; import frc.robot.subsystems.*; @@ -65,8 +66,8 @@ public enum DriveState {Reef, Station, Barge, None} s_Swerve.addVisionMeasurement(poseEst, timestmp); }, () -> Pair.of(s_Swerve.getPigeon2().getYaw().getValueAsDouble(), swerveState.Speeds.omegaRadiansPerSecond), - new Limelight("limelight-fore"), - new Limelight("limelight-aft") + new Limelight(foreLimelightName), + new Limelight(aftLimelightName) ); /* Controllers */ diff --git a/src/main/java/frc/robot/constants/IDConstants.java b/src/main/java/frc/robot/constants/IDConstants.java index 64e30a4..933a0b1 100644 --- a/src/main/java/frc/robot/constants/IDConstants.java +++ b/src/main/java/frc/robot/constants/IDConstants.java @@ -33,6 +33,11 @@ public final class IDConstants public static final int coralRollerID = 13; public static final int coralSensorDIO = 0; - public static final int AlgaeRollerID = 14; - public static final int AlgaeSensorDIO = 1; + public static final int algaeRollerID = 14; + public static final int algaeSensorDIO = 1; + + /* Limelights */ + /* __________ */ + public static final String foreLimelightName = "limelight-fore"; + public static final String aftLimelightName = "limelight-aft"; }