From fabdf4c7c21e4a80408c216db15400f4e89f7ce9 Mon Sep 17 00:00:00 2001 From: kujo27 Date: Sat, 29 Mar 2025 18:21:18 -0700 Subject: [PATCH 01/12] AlignToAlgae PID --- .../subsystems/drive/DriveSubsystem.java | 24 +++++++++++++++++++ ...AlignToNearestReefFaceForAlgaeCommand.java | 15 +++++++++++- 2 files changed, 38 insertions(+), 1 deletion(-) diff --git a/src/main/java/competition/subsystems/drive/DriveSubsystem.java b/src/main/java/competition/subsystems/drive/DriveSubsystem.java index ddbbb2b2a..8ee24cf40 100644 --- a/src/main/java/competition/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/competition/subsystems/drive/DriveSubsystem.java @@ -20,6 +20,7 @@ import xbot.common.injection.swerve.RearLeftDrive; import xbot.common.injection.swerve.RearRightDrive; import xbot.common.injection.swerve.SwerveComponent; +import xbot.common.math.PIDDefaults; import xbot.common.math.PIDManager; import xbot.common.math.PIDManager.PIDManagerFactory; import xbot.common.math.XYPair; @@ -48,6 +49,8 @@ public class DriveSubsystem extends BaseSwerveDriveSubsystem { private final DoubleProperty driveToWaypointsSpeed; private final DoubleProperty driveToWaypointsDurationPerPoint; + private final PIDManager driveToAlgaePidManager; + @Inject public DriveSubsystem(PIDManagerFactory pidFactory, PropertyFactory pf, @FrontLeftDrive SwerveComponent frontLeftSwerve, @FrontRightDrive SwerveComponent frontRightSwerve, @@ -86,6 +89,27 @@ public DriveSubsystem(PIDManagerFactory pidFactory, PropertyFactory pf, driveToWaypointsSpeed = pf.createPersistentProperty("Speed to drive to waypoints", 2); // meters/s driveToWaypointsDurationPerPoint = pf.createPersistentProperty("Time to drive to waypoints", 0.1); // seconds + + + driveToAlgaePidManager = pidFactory.create( + this.getPrefix() + "DriveToAlgaePID", + new PIDDefaults( + 1.08, // P + 0, // I + 4.0, // D + 0.0, // F + 0.6, // Max output + -0.6, // Min output + 0.05, // Error threshold + 0.005, // Derivative threshold + 0.2) // Time threshold + ); + driveToAlgaePidManager.setEnableErrorThreshold(true); + driveToAlgaePidManager.setEnableTimeThreshold(true); + } + + public PIDManager getDriveToAlgaePidManager() { + return driveToAlgaePidManager; } public Translation2d getLookAtPointTarget() { diff --git a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java index e78e9d35a..2c8b94e2c 100644 --- a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java @@ -60,7 +60,7 @@ public void execute() { // Everything below here can and will be extracted because it is repeated code with // DriveWithSnapToReefTagCommand - var centeringTranslation2d = drive.getPowerForRelativePositionChange(new Translation2d(0, robotRelativeTagLocationY)); + var centeringTranslation2d = getPowerForRelativePositionChange(new Translation2d(0, robotRelativeTagLocationY)); XYPair centeringVector = new XYPair(centeringTranslation2d.getX(), centeringTranslation2d.getY()); centeringVector = centeringVector.rotate(pose.getCurrentHeading().getDegrees()); @@ -84,4 +84,17 @@ public void execute() { new XYPair() ); } + + public Translation2d getPowerForRelativePositionChange(Translation2d goalPosition) { + double goalMagnitude = goalPosition.getNorm(); + + if (Math.abs(goalMagnitude) < 0.0001) { + goalMagnitude = 0.0001; + } + Translation2d normalizedGoalVector = goalPosition.div(goalMagnitude); + + double power = -drive.getDriveToAlgaePidManager().calculate(0, goalMagnitude); + + return normalizedGoalVector.times(power); + } } \ No newline at end of file From 379f70d93d95aa2848362b4fe042b9dd7d558063 Mon Sep 17 00:00:00 2001 From: kujo27 Date: Sat, 29 Mar 2025 18:37:53 -0700 Subject: [PATCH 02/12] heading --- .../subsystems/drive/DriveSubsystem.java | 25 +++++++++++++++++-- ...AlignToNearestReefFaceForAlgaeCommand.java | 2 +- 2 files changed, 24 insertions(+), 3 deletions(-) diff --git a/src/main/java/competition/subsystems/drive/DriveSubsystem.java b/src/main/java/competition/subsystems/drive/DriveSubsystem.java index 8ee24cf40..8b3f153b9 100644 --- a/src/main/java/competition/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/competition/subsystems/drive/DriveSubsystem.java @@ -50,6 +50,7 @@ public class DriveSubsystem extends BaseSwerveDriveSubsystem { private final DoubleProperty driveToWaypointsDurationPerPoint; private final PIDManager driveToAlgaePidManager; + private final PIDManager snapToAlgaePIDManager; @Inject public DriveSubsystem(PIDManagerFactory pidFactory, PropertyFactory pf, @@ -92,9 +93,9 @@ public DriveSubsystem(PIDManagerFactory pidFactory, PropertyFactory pf, driveToAlgaePidManager = pidFactory.create( - this.getPrefix() + "DriveToAlgaePID", + this.getPrefix() + "DriveToAlgaePositionalPID", new PIDDefaults( - 1.08, // P + 0.75, // P 0, // I 4.0, // D 0.0, // F @@ -106,12 +107,32 @@ public DriveSubsystem(PIDManagerFactory pidFactory, PropertyFactory pf, ); driveToAlgaePidManager.setEnableErrorThreshold(true); driveToAlgaePidManager.setEnableTimeThreshold(true); + + snapToAlgaePIDManager = pidFactory.create( + this.getPrefix() + "SnapToAlgaeHeadingPID", + new PIDDefaults( + 0.005, // P + 0.000001, // I + 0.02, // D + 0.0, // F + 0.75, // Max output + -0.75, // Min output + 2.0, // Error threshold + 0.2, // Derivative threshold + 0.2) // Time threshold + ); + snapToAlgaePIDManager.setEnableErrorThreshold(true); + snapToAlgaePIDManager.setEnableTimeThreshold(true); } public PIDManager getDriveToAlgaePidManager() { return driveToAlgaePidManager; } + public PIDManager getSnapToAlgaePIDManager() { + return snapToAlgaePIDManager; + } + public Translation2d getLookAtPointTarget() { return lookAtPointTarget; } diff --git a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java index 2c8b94e2c..1c2566461 100644 --- a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java @@ -32,7 +32,7 @@ public class AlignToNearestReefFaceForAlgaeCommand extends BaseCommand { public AlignToNearestReefFaceForAlgaeCommand(HeadingModule.HeadingModuleFactory headingModuleFactory, DriveSubsystem drive, PoseSubsystem pose, AprilTagVisionSubsystemExtended vision, OperatorInterface oi) { - this.headingModule = headingModuleFactory.create(drive.getRotateToHeadingPid()); + this.headingModule = headingModuleFactory.create(drive.getSnapToAlgaePIDManager()); this.vision = vision; this.drive = drive; this.pose = pose; From a72724c1714bc1a6e7590fd9f78b680d9179ab52 Mon Sep 17 00:00:00 2001 From: kujo27 Date: Sat, 29 Mar 2025 19:31:56 -0700 Subject: [PATCH 03/12] default pid --- .../java/competition/subsystems/drive/DriveSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/competition/subsystems/drive/DriveSubsystem.java b/src/main/java/competition/subsystems/drive/DriveSubsystem.java index 8b3f153b9..c8dbf0ccd 100644 --- a/src/main/java/competition/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/competition/subsystems/drive/DriveSubsystem.java @@ -112,8 +112,8 @@ public DriveSubsystem(PIDManagerFactory pidFactory, PropertyFactory pf, this.getPrefix() + "SnapToAlgaeHeadingPID", new PIDDefaults( 0.005, // P - 0.000001, // I - 0.02, // D + 0, // I + 0, // D 0.0, // F 0.75, // Max output -0.75, // Min output From 6436eeec2da3e412b86ec6fcd943faefd40b0e29 Mon Sep 17 00:00:00 2001 From: kujo27 Date: Sat, 29 Mar 2025 20:01:22 -0700 Subject: [PATCH 04/12] deadband --- .../commands/AlignToNearestReefFaceForAlgaeCommand.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java index 1c2566461..cbf093502 100644 --- a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.Angle; import xbot.common.command.BaseCommand; +import xbot.common.math.MathUtils; import xbot.common.math.XYPair; import xbot.common.subsystems.drive.control_logic.HeadingModule; @@ -65,7 +66,10 @@ public void execute() { centeringVector = centeringVector.rotate(pose.getCurrentHeading().getDegrees()); var fieldVectorTranslation2d = oi.driverGamepad.getLeftFieldOrientedVector(); - XYPair fieldVectorXYPair = new XYPair(fieldVectorTranslation2d.getX(), fieldVectorTranslation2d.getY()); + XYPair fieldVectorXYPair = new XYPair( + MathUtils.deadband(fieldVectorTranslation2d.getX(), 0.05), + MathUtils.deadband(fieldVectorTranslation2d.getY(), 0.05) + ); double railsSimilarityToDriver = fieldVectorXYPair.dotProduct( new XYPair( Math.cos(idealFinalHeading.in(Radians) + Math.PI), From eb549ced5817c287a50da9350366e1c0e70045e7 Mon Sep 17 00:00:00 2001 From: kujo27 Date: Sat, 29 Mar 2025 20:07:07 -0700 Subject: [PATCH 05/12] deadband --- .../subsystems/drive/DriveSubsystem.java | 43 ------------------ ...AlignToNearestReefFaceForAlgaeCommand.java | 45 +++++++++++++++++-- 2 files changed, 42 insertions(+), 46 deletions(-) diff --git a/src/main/java/competition/subsystems/drive/DriveSubsystem.java b/src/main/java/competition/subsystems/drive/DriveSubsystem.java index c8dbf0ccd..e6dcb8b3d 100644 --- a/src/main/java/competition/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/competition/subsystems/drive/DriveSubsystem.java @@ -49,8 +49,6 @@ public class DriveSubsystem extends BaseSwerveDriveSubsystem { private final DoubleProperty driveToWaypointsSpeed; private final DoubleProperty driveToWaypointsDurationPerPoint; - private final PIDManager driveToAlgaePidManager; - private final PIDManager snapToAlgaePIDManager; @Inject public DriveSubsystem(PIDManagerFactory pidFactory, PropertyFactory pf, @@ -90,47 +88,6 @@ public DriveSubsystem(PIDManagerFactory pidFactory, PropertyFactory pf, driveToWaypointsSpeed = pf.createPersistentProperty("Speed to drive to waypoints", 2); // meters/s driveToWaypointsDurationPerPoint = pf.createPersistentProperty("Time to drive to waypoints", 0.1); // seconds - - - driveToAlgaePidManager = pidFactory.create( - this.getPrefix() + "DriveToAlgaePositionalPID", - new PIDDefaults( - 0.75, // P - 0, // I - 4.0, // D - 0.0, // F - 0.6, // Max output - -0.6, // Min output - 0.05, // Error threshold - 0.005, // Derivative threshold - 0.2) // Time threshold - ); - driveToAlgaePidManager.setEnableErrorThreshold(true); - driveToAlgaePidManager.setEnableTimeThreshold(true); - - snapToAlgaePIDManager = pidFactory.create( - this.getPrefix() + "SnapToAlgaeHeadingPID", - new PIDDefaults( - 0.005, // P - 0, // I - 0, // D - 0.0, // F - 0.75, // Max output - -0.75, // Min output - 2.0, // Error threshold - 0.2, // Derivative threshold - 0.2) // Time threshold - ); - snapToAlgaePIDManager.setEnableErrorThreshold(true); - snapToAlgaePIDManager.setEnableTimeThreshold(true); - } - - public PIDManager getDriveToAlgaePidManager() { - return driveToAlgaePidManager; - } - - public PIDManager getSnapToAlgaePIDManager() { - return snapToAlgaePIDManager; } public Translation2d getLookAtPointTarget() { diff --git a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java index cbf093502..ed8a212b1 100644 --- a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java @@ -9,6 +9,8 @@ import edu.wpi.first.units.measure.Angle; import xbot.common.command.BaseCommand; import xbot.common.math.MathUtils; +import xbot.common.math.PIDDefaults; +import xbot.common.math.PIDManager; import xbot.common.math.XYPair; import xbot.common.subsystems.drive.control_logic.HeadingModule; @@ -29,16 +31,53 @@ public class AlignToNearestReefFaceForAlgaeCommand extends BaseCommand { Translation2d idealFinalPosition; Angle idealFinalHeading; + private final PIDManager driveToAlgaePidManager; + private final PIDManager snapToAlgaePIDManager; + @Inject public AlignToNearestReefFaceForAlgaeCommand(HeadingModule.HeadingModuleFactory headingModuleFactory, DriveSubsystem drive, PoseSubsystem pose, - AprilTagVisionSubsystemExtended vision, OperatorInterface oi) { - this.headingModule = headingModuleFactory.create(drive.getSnapToAlgaePIDManager()); + AprilTagVisionSubsystemExtended vision, OperatorInterface oi, + PIDManager.PIDManagerFactory pidFactory) { this.vision = vision; this.drive = drive; this.pose = pose; this.oi = oi; this.addRequirements(drive); + + driveToAlgaePidManager = pidFactory.create( + this.getPrefix() + "DriveToAlgaePositionalPID", + new PIDDefaults( + 0.75, // P + 0, // I + 4.0, // D + 0.0, // F + 0.6, // Max output + -0.6, // Min output + 0.05, // Error threshold + 0.005, // Derivative threshold + 0.2) // Time threshold + ); + driveToAlgaePidManager.setEnableErrorThreshold(true); + driveToAlgaePidManager.setEnableTimeThreshold(true); + + snapToAlgaePIDManager = pidFactory.create( + this.getPrefix() + "SnapToAlgaeHeadingPID", + new PIDDefaults( + 0.005, // P + 0, // I + 0, // D + 0.0, // F + 0.75, // Max output + -0.75, // Min output + 2.0, // Error threshold + 0.2, // Derivative threshold + 0.2) // Time threshold + ); + snapToAlgaePIDManager.setEnableErrorThreshold(true); + snapToAlgaePIDManager.setEnableTimeThreshold(true); + + this.headingModule = headingModuleFactory.create(snapToAlgaePIDManager); } @Override @@ -97,7 +136,7 @@ public Translation2d getPowerForRelativePositionChange(Translation2d goalPositio } Translation2d normalizedGoalVector = goalPosition.div(goalMagnitude); - double power = -drive.getDriveToAlgaePidManager().calculate(0, goalMagnitude); + double power = -driveToAlgaePidManager.calculate(0, goalMagnitude); return normalizedGoalVector.times(power); } From b0abc7dbf30abe3720deebdcf2f401da5582dbe6 Mon Sep 17 00:00:00 2001 From: kujo27 Date: Sat, 29 Mar 2025 20:09:37 -0700 Subject: [PATCH 06/12] reset DriveSubsystem --- src/main/java/competition/subsystems/drive/DriveSubsystem.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/competition/subsystems/drive/DriveSubsystem.java b/src/main/java/competition/subsystems/drive/DriveSubsystem.java index e6dcb8b3d..ddbbb2b2a 100644 --- a/src/main/java/competition/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/competition/subsystems/drive/DriveSubsystem.java @@ -20,7 +20,6 @@ import xbot.common.injection.swerve.RearLeftDrive; import xbot.common.injection.swerve.RearRightDrive; import xbot.common.injection.swerve.SwerveComponent; -import xbot.common.math.PIDDefaults; import xbot.common.math.PIDManager; import xbot.common.math.PIDManager.PIDManagerFactory; import xbot.common.math.XYPair; @@ -49,7 +48,6 @@ public class DriveSubsystem extends BaseSwerveDriveSubsystem { private final DoubleProperty driveToWaypointsSpeed; private final DoubleProperty driveToWaypointsDurationPerPoint; - @Inject public DriveSubsystem(PIDManagerFactory pidFactory, PropertyFactory pf, @FrontLeftDrive SwerveComponent frontLeftSwerve, @FrontRightDrive SwerveComponent frontRightSwerve, From aad0f7955fdca30b4f538f127d5edbc6bd1863d9 Mon Sep 17 00:00:00 2001 From: Rongrui Zhu Date: Sun, 30 Mar 2025 17:55:04 -0700 Subject: [PATCH 07/12] Algae alignment for both sides --- ...AlignToNearestReefFaceForAlgaeCommand.java | 12 ++++++- .../subsystems/pose/PoseSubsystem.java | 31 ++++++++++++++----- 2 files changed, 35 insertions(+), 8 deletions(-) diff --git a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java index ed8a212b1..59f468f9e 100644 --- a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.DriverStation; import xbot.common.command.BaseCommand; import xbot.common.math.MathUtils; import xbot.common.math.PIDDefaults; @@ -17,6 +18,7 @@ import javax.inject.Inject; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Radians; public class AlignToNearestReefFaceForAlgaeCommand extends BaseCommand { @@ -84,7 +86,15 @@ public AlignToNearestReefFaceForAlgaeCommand(HeadingModule.HeadingModuleFactory public void initialize() { log.info("Initializing"); - Pose2d closestPose = pose.getClosestReefFacePose(); + // Firstly, figure out if we are on red/blue side of the field, and we'll center accordingly + Pose2d currentPose = pose.getCurrentPose2d(); + + DriverStation.Alliance alliance = DriverStation.Alliance.Blue; + if (currentPose.getTranslation().getX() > PoseSubsystem.fieldXMidpointInMeters.in(Meters)) { + alliance = DriverStation.Alliance.Red; + } + + Pose2d closestPose = pose.getClosestReefFacePoseByAlliance(alliance); idealFinalPosition = closestPose.getTranslation(); idealFinalHeading = closestPose.getRotation().getMeasure().plus(Degrees.of(180)); } diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index 35c148417..1e9a39c4c 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/src/main/java/competition/subsystems/pose/PoseSubsystem.java @@ -25,6 +25,7 @@ import static edu.wpi.first.units.Units.Meters; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -398,16 +399,32 @@ public void ingestSimulatedSwerveModulePositions(SwerveModulePosition[] position this.simulatedModulePositions = Optional.of(positions); } + public static List getReefFacePoses() { + return Arrays.asList( + Landmarks.BlueCloseAlgae, + Landmarks.BlueCloseLeftAlgae, + Landmarks.BlueCloseRightAlgae, + Landmarks.BlueFarLeftAlgae, + Landmarks.BlueFarAlgae, + Landmarks.BlueFarRightAlgae); + } + public Pose2d getClosestReefFacePose() { Pose2d currentPose = getCurrentPose2d(); - List reefFacePoses = Arrays.asList( - convertBlueToRedIfNeeded(Landmarks.BlueCloseAlgae), - convertBlueToRedIfNeeded(Landmarks.BlueCloseLeftAlgae), - convertBlueToRedIfNeeded(Landmarks.BlueCloseRightAlgae), - convertBlueToRedIfNeeded(Landmarks.BlueFarLeftAlgae), - convertBlueToRedIfNeeded(Landmarks.BlueFarAlgae), - convertBlueToRedIfNeeded(Landmarks.BlueFarRightAlgae)); + List reefFacePoses = getReefFacePoses(); + reefFacePoses.replaceAll(PoseSubsystem::convertBlueToRedIfNeeded); + + return currentPose.nearest(reefFacePoses); + } + + public Pose2d getClosestReefFacePoseByAlliance(DriverStation.Alliance alliance) { + Pose2d currentPose = getCurrentPose2d(); + + List reefFacePoses = getReefFacePoses(); + if (alliance == DriverStation.Alliance.Red) { + reefFacePoses.replaceAll(PoseSubsystem::convertBluetoRed); + } return currentPose.nearest(reefFacePoses); } From cae8a72795e71fd2a8b73ec4815accc19ad9cd46 Mon Sep 17 00:00:00 2001 From: Rongrui Zhu Date: Sun, 30 Mar 2025 18:43:53 -0700 Subject: [PATCH 08/12] State machine for algae alignment State machines are very enjoyable to write --- ...AlignToNearestReefFaceForAlgaeCommand.java | 79 ++++++++++++++++++- .../AprilTagVisionSubsystemExtended.java | 13 +++ 2 files changed, 89 insertions(+), 3 deletions(-) diff --git a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java index 59f468f9e..6747c12e8 100644 --- a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java @@ -2,6 +2,8 @@ import competition.operator_interface.OperatorInterface; import competition.subsystems.drive.DriveSubsystem; +import competition.subsystems.oracle.ReefCoordinateGenerator; +import competition.subsystems.pose.Landmarks; import competition.subsystems.pose.PoseSubsystem; import competition.subsystems.vision.AprilTagVisionSubsystemExtended; import edu.wpi.first.math.geometry.Pose2d; @@ -13,6 +15,8 @@ import xbot.common.math.PIDDefaults; import xbot.common.math.PIDManager; import xbot.common.math.XYPair; +import xbot.common.properties.DoubleProperty; +import xbot.common.properties.PropertyFactory; import xbot.common.subsystems.drive.control_logic.HeadingModule; import javax.inject.Inject; @@ -29,6 +33,7 @@ public class AlignToNearestReefFaceForAlgaeCommand extends BaseCommand { final DriveSubsystem drive; final PoseSubsystem pose; final OperatorInterface oi; + final ReefCoordinateGenerator reefCoordinateGenerator; Translation2d idealFinalPosition; Angle idealFinalHeading; @@ -36,15 +41,29 @@ public class AlignToNearestReefFaceForAlgaeCommand extends BaseCommand { private final PIDManager driveToAlgaePidManager; private final PIDManager snapToAlgaePIDManager; + final DoubleProperty interstitialThresholdToRailDrive; + final DoubleProperty interstitialDistance; + final DoubleProperty interstitialApproachSpeedFactor; + + public enum AlignmentState { + ToInterstitial, + RailDrive, + } + + public AlignmentState currentAlignmentState; + public Pose2d interstitialPoint; + @Inject public AlignToNearestReefFaceForAlgaeCommand(HeadingModule.HeadingModuleFactory headingModuleFactory, DriveSubsystem drive, PoseSubsystem pose, AprilTagVisionSubsystemExtended vision, OperatorInterface oi, - PIDManager.PIDManagerFactory pidFactory) { + PIDManager.PIDManagerFactory pidFactory, PropertyFactory pf, + ReefCoordinateGenerator reefCoordinateGenerator) { this.vision = vision; this.drive = drive; this.pose = pose; this.oi = oi; + this.reefCoordinateGenerator = reefCoordinateGenerator; this.addRequirements(drive); driveToAlgaePidManager = pidFactory.create( @@ -80,6 +99,10 @@ public AlignToNearestReefFaceForAlgaeCommand(HeadingModule.HeadingModuleFactory snapToAlgaePIDManager.setEnableTimeThreshold(true); this.headingModule = headingModuleFactory.create(snapToAlgaePIDManager); + + interstitialThresholdToRailDrive = pf.createPersistentProperty("InterstitialThresholdToRailDrive-m", 0.2); + interstitialDistance = pf.createPersistentProperty("InterstitialDistance-m", 1.75); + interstitialApproachSpeedFactor = pf.createPersistentProperty("InterstitialApproachSpeedFactor", 0.5); } @Override @@ -96,11 +119,61 @@ public void initialize() { Pose2d closestPose = pose.getClosestReefFacePoseByAlliance(alliance); idealFinalPosition = closestPose.getTranslation(); - idealFinalHeading = closestPose.getRotation().getMeasure().plus(Degrees.of(180)); + idealFinalHeading = closestPose.getRotation().getMeasure().plus(Degrees.of(180)); // Aligning backwards + + double distanceToClosestReefFace = currentPose.getTranslation().getDistance(idealFinalPosition); + + // Determine our starting state, do we need to drive to an interstitial? + if (distanceToClosestReefFace < interstitialDistance.get()) { + currentAlignmentState = AlignmentState.RailDrive; + } else { + currentAlignmentState = AlignmentState.ToInterstitial; + interstitialPoint = reefCoordinateGenerator.getPoseRelativeToReefFace( + alliance, + Landmarks.getReefFaceFromTagId(vision.getClosestTagIdFromTranslation(idealFinalPosition)), + Meters.of(interstitialDistance.get()), + Meters.zero() + ); + } } @Override public void execute() { + switch (currentAlignmentState) { + case ToInterstitial -> { + driveToInterstitial(); + var currentTranslation = pose.getCurrentPose2d().getTranslation(); + double distanceFromInterstitial = currentTranslation.getDistance(interstitialPoint.getTranslation()); + if (distanceFromInterstitial < interstitialThresholdToRailDrive.get()) { + currentAlignmentState = AlignmentState.RailDrive; + } + } + case RailDrive -> railDrive(); + default -> {} + } + } + + public void driveToInterstitial() { + Pose2d currentPose = pose.getCurrentPose2d(); + + // Same interstitial driving logic as AlignCameraToAprilTagCalculator + var vectorTowardsInterstitial = interstitialPoint.getTranslation().minus(currentPose.getTranslation()); + var normalizedVector = vectorTowardsInterstitial.div(vectorTowardsInterstitial.getNorm()); + var driveIntent = new XYPair( + normalizedVector.getX(), + normalizedVector.getY()).scale(interstitialApproachSpeedFactor.get() + ); + var rotationIntent = headingModule.calculateHeadingPower(interstitialPoint.getRotation()); + + drive.fieldOrientedDrive( + driveIntent, + rotationIntent, + pose.getCurrentHeading().getDegrees(), + new XYPair() + ); + } + + private void railDrive() { // Calculate the vector to center our robot on "rails" var currentTranslation = pose.getCurrentPose2d().getTranslation(); var currentHeading = pose.getCurrentHeading().getRadians(); @@ -128,7 +201,7 @@ public void execute() { double rotateIntent = headingModule.calculateHeadingPower(idealFinalHeading.in(Degrees)); XYPair onRailsVector = XYPair.fromPolar(idealFinalHeading.in(Degrees), railsSimilarityToDriver); - var combinedVector = onRailsVector.clone().add(centeringVector); + var combinedVector = onRailsVector.add(centeringVector); drive.fieldOrientedDrive( combinedVector, diff --git a/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java b/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java index e9e562589..d6d844a0d 100644 --- a/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java +++ b/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java @@ -96,6 +96,19 @@ public int getTargetAprilTagID(Landmarks.ReefFace reefFace) { return getTargetAprilTagID(targetReefFacePose); } + public int getClosestTagIdFromTranslation(Translation2d translation) { + double minDistance = Double.MAX_VALUE; + int closestTagId = -1; + for (Pose2d reefFacePose : aprilTagIDHashMap.keySet()) { + if (translation.getDistance(reefFacePose.getTranslation()) < minDistance) { + minDistance = translation.getDistance(reefFacePose.getTranslation()); + closestTagId = aprilTagIDHashMap.get(reefFacePose); + } + } + + return closestTagId; + } + public boolean areAllCamerasConnected() { for (int i = 0; i < cameras.length; i++) { if (!isCameraConnected(i)) { From bc5ce4e313a4989f66b3d0273f04f8df0546b506 Mon Sep 17 00:00:00 2001 From: Rongrui Zhu Date: Sun, 30 Mar 2025 18:51:55 -0700 Subject: [PATCH 09/12] Backwards interstitial and pf fix --- .../commands/AlignToNearestReefFaceForAlgaeCommand.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java index 6747c12e8..cd9fff268 100644 --- a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java @@ -7,6 +7,7 @@ import competition.subsystems.pose.PoseSubsystem; import competition.subsystems.vision.AprilTagVisionSubsystemExtended; 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.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation; @@ -100,6 +101,7 @@ public AlignToNearestReefFaceForAlgaeCommand(HeadingModule.HeadingModuleFactory this.headingModule = headingModuleFactory.create(snapToAlgaePIDManager); + pf.setPrefix(this.getPrefix()); interstitialThresholdToRailDrive = pf.createPersistentProperty("InterstitialThresholdToRailDrive-m", 0.2); interstitialDistance = pf.createPersistentProperty("InterstitialDistance-m", 1.75); interstitialApproachSpeedFactor = pf.createPersistentProperty("InterstitialApproachSpeedFactor", 0.5); @@ -134,6 +136,12 @@ public void initialize() { Meters.of(interstitialDistance.get()), Meters.zero() ); + + // Recall how we are aligning backwards + interstitialPoint = new Pose2d( + interstitialPoint.getTranslation(), + interstitialPoint.getRotation().plus(new Rotation2d(Math.PI)) + ); } } @@ -151,6 +159,7 @@ public void execute() { case RailDrive -> railDrive(); default -> {} } + aKitLog.record("AlignmentState", currentAlignmentState); } public void driveToInterstitial() { From 9df03777ad019d46df3f191baf4b130f79d86db5 Mon Sep 17 00:00:00 2001 From: Rongrui Zhu Date: Mon, 31 Mar 2025 19:03:48 -0700 Subject: [PATCH 10/12] Resolved PR comments Distance Property + renaming --- ...AlignToNearestReefFaceForAlgaeCommand.java | 21 ++++++++++--------- .../subsystems/pose/PoseSubsystem.java | 8 +++---- .../AprilTagVisionSubsystemExtended.java | 2 +- 3 files changed, 15 insertions(+), 16 deletions(-) diff --git a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java index cd9fff268..1521089ce 100644 --- a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java @@ -16,6 +16,7 @@ import xbot.common.math.PIDDefaults; import xbot.common.math.PIDManager; import xbot.common.math.XYPair; +import xbot.common.properties.DistanceProperty; import xbot.common.properties.DoubleProperty; import xbot.common.properties.PropertyFactory; import xbot.common.subsystems.drive.control_logic.HeadingModule; @@ -42,8 +43,8 @@ public class AlignToNearestReefFaceForAlgaeCommand extends BaseCommand { private final PIDManager driveToAlgaePidManager; private final PIDManager snapToAlgaePIDManager; - final DoubleProperty interstitialThresholdToRailDrive; - final DoubleProperty interstitialDistance; + final DistanceProperty interstitialThresholdToRailDrive; + final DistanceProperty interstitialDistance; final DoubleProperty interstitialApproachSpeedFactor; public enum AlignmentState { @@ -102,18 +103,18 @@ public AlignToNearestReefFaceForAlgaeCommand(HeadingModule.HeadingModuleFactory this.headingModule = headingModuleFactory.create(snapToAlgaePIDManager); pf.setPrefix(this.getPrefix()); - interstitialThresholdToRailDrive = pf.createPersistentProperty("InterstitialThresholdToRailDrive-m", 0.2); - interstitialDistance = pf.createPersistentProperty("InterstitialDistance-m", 1.75); + interstitialThresholdToRailDrive = pf.createPersistentProperty("InterstitialThresholdToRailDrive", Meters.of(0.2)); + interstitialDistance = pf.createPersistentProperty("InterstitialDistance", Meters.of(1.75)); interstitialApproachSpeedFactor = pf.createPersistentProperty("InterstitialApproachSpeedFactor", 0.5); } @Override public void initialize() { log.info("Initializing"); - - // Firstly, figure out if we are on red/blue side of the field, and we'll center accordingly Pose2d currentPose = pose.getCurrentPose2d(); + // Firstly, figure out if we are on red/blue side of the field, and we'll center accordingly + // There exist a possibility that we want to steal algae on the opposition's reef (although it goes both ways) DriverStation.Alliance alliance = DriverStation.Alliance.Blue; if (currentPose.getTranslation().getX() > PoseSubsystem.fieldXMidpointInMeters.in(Meters)) { alliance = DriverStation.Alliance.Red; @@ -126,14 +127,14 @@ public void initialize() { double distanceToClosestReefFace = currentPose.getTranslation().getDistance(idealFinalPosition); // Determine our starting state, do we need to drive to an interstitial? - if (distanceToClosestReefFace < interstitialDistance.get()) { + if (distanceToClosestReefFace < interstitialDistance.get().in(Meters)) { currentAlignmentState = AlignmentState.RailDrive; } else { currentAlignmentState = AlignmentState.ToInterstitial; interstitialPoint = reefCoordinateGenerator.getPoseRelativeToReefFace( alliance, - Landmarks.getReefFaceFromTagId(vision.getClosestTagIdFromTranslation(idealFinalPosition)), - Meters.of(interstitialDistance.get()), + Landmarks.getReefFaceFromTagId(vision.getClosestReefTagIdFromTranslation(idealFinalPosition)), + interstitialDistance.get(), Meters.zero() ); @@ -152,7 +153,7 @@ public void execute() { driveToInterstitial(); var currentTranslation = pose.getCurrentPose2d().getTranslation(); double distanceFromInterstitial = currentTranslation.getDistance(interstitialPoint.getTranslation()); - if (distanceFromInterstitial < interstitialThresholdToRailDrive.get()) { + if (distanceFromInterstitial < interstitialThresholdToRailDrive.get().in(Meters)) { currentAlignmentState = AlignmentState.RailDrive; } } diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index 1e9a39c4c..189fd392f 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/src/main/java/competition/subsystems/pose/PoseSubsystem.java @@ -14,7 +14,6 @@ import competition.subsystems.drive.DriveSubsystem; import competition.subsystems.vision.AprilTagVisionSubsystemExtended; import competition.subsystems.vision.CoprocessorCommunicationSubsystem; -import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator; import edu.wpi.first.math.estimator.PoseEstimator; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -40,7 +39,6 @@ import xbot.common.properties.Property; import xbot.common.properties.PropertyFactory; import xbot.common.subsystems.pose.BasePoseSubsystem; -import xbot.common.subsystems.vision.AprilTagVisionSubsystem; @Singleton public class PoseSubsystem extends BasePoseSubsystem { @@ -399,7 +397,7 @@ public void ingestSimulatedSwerveModulePositions(SwerveModulePosition[] position this.simulatedModulePositions = Optional.of(positions); } - public static List getReefFacePoses() { + public static List getBlueReefFacePoses() { return Arrays.asList( Landmarks.BlueCloseAlgae, Landmarks.BlueCloseLeftAlgae, @@ -412,7 +410,7 @@ public static List getReefFacePoses() { public Pose2d getClosestReefFacePose() { Pose2d currentPose = getCurrentPose2d(); - List reefFacePoses = getReefFacePoses(); + List reefFacePoses = getBlueReefFacePoses(); reefFacePoses.replaceAll(PoseSubsystem::convertBlueToRedIfNeeded); return currentPose.nearest(reefFacePoses); @@ -421,7 +419,7 @@ public Pose2d getClosestReefFacePose() { public Pose2d getClosestReefFacePoseByAlliance(DriverStation.Alliance alliance) { Pose2d currentPose = getCurrentPose2d(); - List reefFacePoses = getReefFacePoses(); + List reefFacePoses = getBlueReefFacePoses(); if (alliance == DriverStation.Alliance.Red) { reefFacePoses.replaceAll(PoseSubsystem::convertBluetoRed); } diff --git a/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java b/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java index d6d844a0d..81cff064b 100644 --- a/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java +++ b/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java @@ -96,7 +96,7 @@ public int getTargetAprilTagID(Landmarks.ReefFace reefFace) { return getTargetAprilTagID(targetReefFacePose); } - public int getClosestTagIdFromTranslation(Translation2d translation) { + public int getClosestReefTagIdFromTranslation(Translation2d translation) { double minDistance = Double.MAX_VALUE; int closestTagId = -1; for (Pose2d reefFacePose : aprilTagIDHashMap.keySet()) { From d170d25e5ae8c61eb4d754ad0e8ed4eb75eb9da5 Mon Sep 17 00:00:00 2001 From: Rong Date: Tue, 1 Apr 2025 19:22:18 -0700 Subject: [PATCH 11/12] Update SeriouslyCommonLib --- SeriouslyCommonLib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index 1f2a534e4..728d9036a 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit 1f2a534e4bbffb9fbb22e77b895b7fa2d9bbd81c +Subproject commit 728d9036a672d9f762eb666c1e400c8652b0ab29 From e97a4e8497046141624b3070f926d6e9d0be4b96 Mon Sep 17 00:00:00 2001 From: Rong Date: Tue, 1 Apr 2025 19:30:32 -0700 Subject: [PATCH 12/12] Removed repeated code --- SeriouslyCommonLib | 2 +- ...AlignToNearestReefFaceForAlgaeCommand.java | 20 +++++-------------- 2 files changed, 6 insertions(+), 16 deletions(-) diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index 728d9036a..6be1c5f86 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit 728d9036a672d9f762eb666c1e400c8652b0ab29 +Subproject commit 6be1c5f86abe27a70331312291e0a36b03906d95 diff --git a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java index 1521089ce..af920db95 100644 --- a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java @@ -191,9 +191,12 @@ private void railDrive() { double deltaY = idealFinalPosition.getY() - currentTranslation.getY(); double robotRelativeTagLocationY = deltaX * Math.sin(-currentHeading) + deltaY * Math.cos(-currentHeading); - // Everything below here can and will be extracted because it is repeated code with + // Everything below here can and should be extracted because it is repeated code with // DriveWithSnapToReefTagCommand - var centeringTranslation2d = getPowerForRelativePositionChange(new Translation2d(0, robotRelativeTagLocationY)); + var centeringTranslation2d = drive.getPowerForRelativePositionChange( + driveToAlgaePidManager, + new Translation2d(0, robotRelativeTagLocationY) + ); XYPair centeringVector = new XYPair(centeringTranslation2d.getX(), centeringTranslation2d.getY()); centeringVector = centeringVector.rotate(pose.getCurrentHeading().getDegrees()); @@ -220,17 +223,4 @@ private void railDrive() { new XYPair() ); } - - public Translation2d getPowerForRelativePositionChange(Translation2d goalPosition) { - double goalMagnitude = goalPosition.getNorm(); - - if (Math.abs(goalMagnitude) < 0.0001) { - goalMagnitude = 0.0001; - } - Translation2d normalizedGoalVector = goalPosition.div(goalMagnitude); - - double power = -driveToAlgaePidManager.calculate(0, goalMagnitude); - - return normalizedGoalVector.times(power); - } } \ No newline at end of file