From fabdf4c7c21e4a80408c216db15400f4e89f7ce9 Mon Sep 17 00:00:00 2001 From: kujo27 Date: Sat, 29 Mar 2025 18:21:18 -0700 Subject: [PATCH 1/6] 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 2/6] 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 3/6] 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 4/6] 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 5/6] 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 6/6] 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,