Subclass HeadingToTargetCommand in ProfiledPointToPointCommand to DRY…

… heading maintain code

src/main/java/frc/robot/commands/auto/ProfiledPointToPointCommand.java

@@ -5,12 +5,11 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.Utilities;
+import frc.robot.commands.HeadingToTargetCommand;
 import frc.robot.commands.interfaces.AutoDrivableCommand;
 import frc.robot.coordinates.PolarCoordinate;
 import frc.robot.subsystems.AutoDrive;
@@ -20,7 +19,7 @@
  * Drive a certain distance from a point - regardless of angle - and maintain that
  * distance away from the point.
  */
-public class ProfiledPointToPointCommand extends CommandBase implements AutoDrivableCommand {
+public class ProfiledPointToPointCommand extends HeadingToTargetCommand implements AutoDrivableCommand {
 
   private PolarCoordinate target;
   private Supplier<Pose2d> poseSupplier;
@@ -34,6 +33,12 @@ public class ProfiledPointToPointCommand extends CommandBase implements AutoDriv
   private double strafeOutput = 0.0;
 
   public ProfiledPointToPointCommand(PolarCoordinate target, Supplier<Pose2d> poseSupplier, Heading heading, AutoDrive autoDrive, double driveP, double strafeP, double forwardAcceleration, double strafeAcceleration) {
+     super(
+      target.toFieldCoordinate(),
+      () -> poseSupplier.get().getTranslation(),
+      heading
+    );
+
     this.target = target;
     this.poseSupplier = poseSupplier;
     this.heading = heading;
@@ -51,11 +56,13 @@ public ProfiledPointToPointCommand(PolarCoordinate target, Supplier<Pose2d> pose
     SmartDashboard.putNumber("ProfiledP2P/Target Distance (inches)", Units.metersToInches(target.getRadiusMeters()));
     SmartDashboard.putNumber("ProfiledP2P/Target Theta (Degrees)", target.getTheta().getDegrees());
 
-    addRequirements(heading, autoDrive);
+    addRequirements(autoDrive);
   }
 
   @Override
   public void initialize() {
+    super.initialize();
+
     heading.enableMaintainHeading();
     autoDrive.setDelegate(this);
 
@@ -79,6 +86,8 @@ private PolarCoordinate getRobotCoordinate() {
 
   @Override
   public void execute() {
+    super.execute();
+
     PolarCoordinate robotCoordinate = getRobotCoordinate();
     forwardOutput = distanceController.calculate(
       robotCoordinate.getRadiusMeters(),
@@ -108,18 +117,9 @@ public void execute() {
     );
 
     log(robotCoordinate);
-
-    // TODO: DRY this, since we use this elsewhere...
-    Pose2d pose = poseSupplier.get();
-    double x = pose.getX() - target.getReferencePoint().getX();
-    double y = pose.getY() - target.getReferencePoint().getY();
-    double towardsCenterDegrees = Math.atan2(y, x);
-    Rotation2d desiredRotation = Rotation2d.fromDegrees(Units.radiansToDegrees(towardsCenterDegrees) - 180);
-    heading.setMaintainHeading(desiredRotation);
   }
 
   public AutoDrive.State calculate(double forward, double strafe, boolean isFieldOriented) {
-    // Note that this command assumes we're facing the target (use with Heading)
     return new AutoDrive.State(
       -forwardOutput,
       strafeOutput,
@@ -129,6 +129,8 @@ public AutoDrive.State calculate(double forward, double strafe, boolean isFieldO
 
   @Override
   public void end(boolean interrupted) {
+    super.end(interrupted);
+
     autoDrive.clearDelegate();
   }