Adds flare command for approaching target at distance

src/main/java/frc/robot/commands/teleop/DistanceToTargetCommand.java

@@ -0,0 +1,124 @@
+package frc.robot.commands.teleop;
+
+import java.util.function.Supplier;
+
+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.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
+import frc.robot.Constants;
+import frc.robot.commands.OdometryHeadingCommand;
+import frc.robot.commands.interfaces.AutoDrivableCommand;
+import frc.robot.coordinates.PolarCoordinate;
+import frc.robot.subsystems.AutoDrive;
+import frc.robot.subsystems.Heading;
+
+/**
+ * Drive a certain distance from a point - regardless of angle - and maintain that
+ * distance away from the point. Will attempt to face target while driving towards
+ * target.
+ */
+public class DistanceToTargetCommand extends OdometryHeadingCommand implements AutoDrivableCommand {
+
+  private Translation2d targetMeters;
+  private double distanceMeters;
+  private Supplier<Pose2d> poseSupplier;
+  private Supplier<ChassisSpeeds> chassisSpeedsProvider;
+  private AutoDrive autoDrive;
+
+  private ProfiledPIDController distanceController;
+
+  private double output = 0.0;
+
+  public DistanceToTargetCommand(double distanceMeters, Supplier<Pose2d> poseSupplier, Supplier<ChassisSpeeds> chassisSpeedsProvider, AutoDrive autoDrive, Heading heading) {
+    this(Constants.kHub, distanceMeters, poseSupplier, chassisSpeedsProvider, autoDrive, heading);
+  }
+
+  public DistanceToTargetCommand(Translation2d targetMeters, double distanceMeters, Supplier<Pose2d> poseSupplier, Supplier<ChassisSpeeds> chassisSpeedsProvider, AutoDrive autoDrive, Heading heading) {
+    super(targetMeters, poseSupplier, heading);
+
+    this.targetMeters = targetMeters;
+    this.distanceMeters = distanceMeters;
+    this.poseSupplier = poseSupplier;
+    this.chassisSpeedsProvider = chassisSpeedsProvider;
+    this.autoDrive = autoDrive;
+
+    final double maxSpeed = 0.8; // 80% of maximum speed
+    final double maxVelocityMetersPerSecond = Constants.Swerve.MAX_VELOCITY_METERS_PER_SECOND * maxSpeed;
+    distanceController = new ProfiledPIDController(3, 0.0, 0.0,
+      // Use max speed above. Come up to max speed in one second
+      new TrapezoidProfile.Constraints(
+        maxVelocityMetersPerSecond,
+        maxVelocityMetersPerSecond 
+      )
+    );
+    // Tolerance for our distance is within ~2 inches (should be Good Enough™️)
+    distanceController.setTolerance(Units.inchesToMeters(2));
+
+    addRequirements(autoDrive);
+  }
+
+  @Override
+  public void initialize() {
+    super.initialize();
+
+    autoDrive.setDelegate(this);
+
+    // Set our initial setpoint for our profiled PID controllers
+    // to avoid a JUMP to their starting values on first run
+    PolarCoordinate robotCoordinate = getRobotCoordinate();
+    distanceController.reset(
+      robotCoordinate.getRadiusMeters(),
+      chassisSpeedsProvider.get().vxMetersPerSecond
+    );
+  }
+
+  @Override
+  public void execute() {
+    super.execute();
+
+    PolarCoordinate robotCoordinate = getRobotCoordinate();
+    output = distanceController.calculate(
+      robotCoordinate.getRadiusMeters(),
+      distanceMeters
+    );
+
+    // Clamp to some max speed (should be between [0.0, 1.0])
+    final double maxSpeed = 1.0;
+    output = MathUtil.clamp(
+      output,
+      -maxSpeed,
+      maxSpeed
+    );
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    super.end(interrupted);
+
+    autoDrive.clearDelegate();
+  }
+
+  private PolarCoordinate getRobotCoordinate() {
+    // Find our robot polar coordinate relative to the target
+    return PolarCoordinate.fromFieldCoordinate(
+      poseSupplier.get().getTranslation(),
+      targetMeters
+    );
+  }
+
+  public AutoDrive.State calculate(double forward, double strafe, boolean isFieldOriented) {
+    // Use our distanController output calculation as our forward value.
+    // Driver can strafe during this command. Forward should be forward to the robot.
+    // Note that this command assumes we're facing the target (use with Heading)
+    return new AutoDrive.State(
+      -output,
+      strafe,
+      false
+    );
+  }
+
+}