diff --git a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java index e78e9d35..ed8a212b 100644 --- a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java @@ -8,6 +8,9 @@ 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.PIDDefaults; +import xbot.common.math.PIDManager; import xbot.common.math.XYPair; import xbot.common.subsystems.drive.control_logic.HeadingModule; @@ -28,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.getRotateToHeadingPid()); + 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 @@ -60,12 +100,15 @@ 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()); 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), @@ -84,4 +127,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 = -driveToAlgaePidManager.calculate(0, goalMagnitude); + + return normalizedGoalVector.times(power); + } } \ No newline at end of file