diff --git a/src/main/java/competition/subsystems/drive/DriveSubsystem.java b/src/main/java/competition/subsystems/drive/DriveSubsystem.java index b54d194f..2c6514e2 100644 --- 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; + } + + } \ No newline at end of file diff --git a/src/main/java/competition/subsystems/drive/commands/PointAtNoteCommand.java b/src/main/java/competition/subsystems/drive/commands/PointAtNoteCommand.java index 95b4efa3..ed246012 100644 --- 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 index 05fa5758..65ab8cc8 100644 --- 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 index b007ae68..261ff592 100644 --- 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 index 4d9672ff..273f895f 100644 --- 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);