Merge branch 'main' into point-at-note-with-bearing-improvements

Team488 · Mar 30, 2024 · 980438f · 980438f
2 parents c0b2589 + 1d7a9a8
commit 980438f
Show file tree

Hide file tree

Showing 5 changed files with 31 additions and 5 deletions.
diff --git a/src/main/java/competition/subsystems/drive/DriveSubsystem.java b/src/main/java/competition/subsystems/drive/DriveSubsystem.java
@@ -15,6 +15,7 @@
 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.properties.DoubleProperty;
 import xbot.common.properties.Property;
@@ -37,11 +38,13 @@ public class DriveSubsystem extends BaseSwerveDriveSubsystem implements DataFram
     private Translation2d specialPointAtPositionTarget = new Translation2d();
     private final DoubleProperty suggestedAutonomousMaximumSpeed;
     private final DoubleProperty suggestedAutonomousExtremeSpeed;
+    private final PIDManager aggressiveGoalHeadingPidManager;
 
     @Inject
     public DriveSubsystem(PIDManagerFactory pidFactory, PropertyFactory pf,
                           @FrontLeftDrive SwerveComponent frontLeftSwerve, @FrontRightDrive SwerveComponent frontRightSwerve,
-                          @RearLeftDrive SwerveComponent rearLeftSwerve, @RearRightDrive SwerveComponent rearRightSwerve) {
+                          @RearLeftDrive SwerveComponent rearLeftSwerve, @RearRightDrive SwerveComponent rearRightSwerve,
+                          PIDManagerFactory aggressiveGoalHeadingPidFactory) {
         super(pidFactory, pf, frontLeftSwerve, frontRightSwerve, rearLeftSwerve, rearRightSwerve);
         log.info("Creating DriveSubsystem");
 
@@ -51,6 +54,22 @@ public DriveSubsystem(PIDManagerFactory pidFactory, PropertyFactory pf,
                 pf.createPersistentProperty("Suggested Autonomous Maximum Speed", 3.0);
         suggestedAutonomousExtremeSpeed =
                 pf.createPersistentProperty("Suggested Autonomous EXTREME Speed", 5.0);
+
+        aggressiveGoalHeadingPidManager = aggressiveGoalHeadingPidFactory.create(
+                this.getPrefix() + "AggressiveGoalHeadingPID",
+                new PIDDefaults(
+                2.16, // 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)
+        );
+        aggressiveGoalHeadingPidManager.setEnableErrorThreshold(true);
+        aggressiveGoalHeadingPidManager.setEnableTimeThreshold(true);
     }
 
     public double getSuggestedAutonomousMaximumSpeed() {
@@ -130,4 +149,9 @@ public InstantCommand createClearAllSpecialTargetsCommand() {
         });
     }
 
+    public PIDManager getAggressiveGoalHeadingPid() {
+        return aggressiveGoalHeadingPidManager;
+    }
+
+
 }
diff --git a/src/main/java/competition/subsystems/drive/commands/PointAtNoteCommand.java b/src/main/java/competition/subsystems/drive/commands/PointAtNoteCommand.java
@@ -39,7 +39,7 @@ public class PointAtNoteCommand extends BaseCommand {
     public PointAtNoteCommand(DriveSubsystem drive, HeadingModule.HeadingModuleFactory headingModuleFactory, PoseSubsystem pose,
                               OperatorInterface oi, DynamicOracle oracle, PropertyFactory pf) {
         this.drive = drive;
-        this.headingModule = headingModuleFactory.create(drive.getRotateToHeadingPid());
+        this.headingModule = headingModuleFactory.create(drive.getAggressiveGoalHeadingPid());
         this.pose = pose;
         this.oi = oi;
         this.oracle = oracle;

diff --git a/src/main/java/competition/subsystems/drive/commands/PointAtNoteWithBearingCommand.java b/src/main/java/competition/subsystems/drive/commands/PointAtNoteWithBearingCommand.java
@@ -45,7 +45,7 @@ public PointAtNoteWithBearingCommand(DriveSubsystem drive, HeadingModule.Heading
                                          OperatorInterface oi, VisionSubsystem vision, PropertyFactory pf, CollectorSubsystem collector,
                                          DynamicOracle oracle) {
         this.drive = drive;
-        this.headingModule = headingModuleFactory.create(drive.getRotateToHeadingPid());
+        this.headingModule = headingModuleFactory.create(drive.getAggressiveGoalHeadingPid());
         this.pose = pose;
         this.oi = oi;
         this.vision = vision;

diff --git a/src/main/java/competition/subsystems/drive/commands/PointAtSpeakerCommand.java b/src/main/java/competition/subsystems/drive/commands/PointAtSpeakerCommand.java
@@ -32,7 +32,7 @@ public class PointAtSpeakerCommand extends BaseCommand {
     public PointAtSpeakerCommand(DriveSubsystem drive, HeadingModule.HeadingModuleFactory headingModuleFactory, PoseSubsystem pose,
                                  OperatorInterface oi, PropertyFactory pf) {
         this.drive = drive;
-        this.headingModule = headingModuleFactory.create(drive.getRotateToHeadingPid());
+        this.headingModule = headingModuleFactory.create(drive.getAggressiveGoalHeadingPid());
         this.pose = pose;
         this.oi = oi;
 //        this.turnPowerFactor = pf.createPersistentProperty("Turn Power Factor", 0.75);

diff --git a/src/main/java/competition/subsystems/drive/commands/SwerveDriveWithJoysticksCommand.java b/src/main/java/competition/subsystems/drive/commands/SwerveDriveWithJoysticksCommand.java
@@ -37,6 +37,7 @@ public class SwerveDriveWithJoysticksCommand extends BaseCommand {
     final DoubleProperty turnPowerFactor;
     boolean absoluteOrientationMode;
     final HeadingModule headingModule;
+    final HeadingModule aggressiveHeadingModule;
     final Latch absoluteOrientationLatch;
     double minimumMagnitudeForAbsoluteHeading;
     final DoubleProperty triggerOnlyPowerScaling;
@@ -59,6 +60,7 @@ public SwerveDriveWithJoysticksCommand(
         this.minimumMagnitudeForAbsoluteHeading = 0.75;
         this.decider = hvmFactory.create(this.getPrefix());
         this.headingModule = headingModuleFactory.create(drive.getRotateToHeadingPid());
+        this.aggressiveHeadingModule = headingModuleFactory.create(drive.getAggressiveGoalHeadingPid());
         this.triggerOnlyPowerScaling = pf.createPersistentProperty("TriggerOnlyPowerScaling", 0.75);
         this.triggerOnlyExponent = pf.createPersistentProperty("TriggerOnlyExponent", 2.0);
 
@@ -205,7 +207,7 @@ private double getSuggestedRotateIntentForAbsoluteStickControl(double humanRotat
         else if (drive.isSpecialPointAtPositionTargetActive()) {
             desiredHeading = getRotationIntentPointAtSpecialPoint();
             drive.setDesiredHeading(desiredHeading);
-            suggestedRotatePower = headingModule.calculateHeadingPower(desiredHeading);
+            suggestedRotatePower = aggressiveHeadingModule.calculateHeadingPower(desiredHeading);
         } else if (drive.isSpecialHeadingTargetActive()) {
             desiredHeading = drive.getSpecialHeadingTarget().getDegrees();
             drive.setDesiredHeading(desiredHeading);