Adds flare command for approaching target at distance

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

@@ -0,0 +1,103 @@
+package frc.robot.commands.teleop;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+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 ParallelCommandGroup {
+
+  public DistanceToTargetCommand(double distanceMeters, Supplier<Pose2d> poseSupplier, AutoDrive autoDrive, Heading heading) {
+    this(Constants.kHub, distanceMeters, poseSupplier, autoDrive, heading);
+  }
+
+  public DistanceToTargetCommand(Translation2d targetMeters, double distanceMeters, Supplier<Pose2d> poseSupplier, AutoDrive autoDrive, Heading heading) {
+    super(
+      new DistanceToPointCommand(targetMeters, distanceMeters, poseSupplier, autoDrive),
+      new OdometryHeadingCommand(targetMeters, poseSupplier, heading)
+    );
+  }
+
+}
+
+class DistanceToPointCommand extends CommandBase implements AutoDrivableCommand {
+  private Translation2d targetMeters;
+  private double distanceMeters;
+  private Supplier<Pose2d> poseSupplier;
+  private AutoDrive autoDrive;
+
+  private PIDController distanController = new PIDController(1.0, 0, 0);
+
+  private double output = 0.0;
+
+  public DistanceToPointCommand(Translation2d targetMeters, double distanceMeters, Supplier<Pose2d> poseSupplier, AutoDrive autoDrive) {
+    this.targetMeters = targetMeters;
+    this.distanceMeters = distanceMeters;
+    this.poseSupplier = poseSupplier;
+    this.autoDrive = autoDrive;
+
+    addRequirements(autoDrive);
+  }
+
+  @Override
+  public void initialize() {
+    autoDrive.setDelegate(this);
+  }
+
+  @Override
+  public void execute() {
+    // Find our robot polar coordinate relative to the target
+    PolarCoordinate robotCoordinate = PolarCoordinate.fromFieldCoordinate(
+      poseSupplier.get().getTranslation(),
+      targetMeters
+    );
+    output = distanController.calculate(
+      robotCoordinate.getRadiusMeters(),
+      distanceMeters
+    );
+
+    // Clamp to some max speed (should be between [0.0, 1.0])
+    final double maxSpeed = 0.3;
+    output = MathUtil.clamp(
+      output,
+      -maxSpeed,
+      maxSpeed
+    );
+
+    // TODO: This should probably be a field-oriented movement based on the
+    // robot's heading. This will allow us to move indepdently of the heading.
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    autoDrive.clearDelegate();
+  }
+
+  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
+    );
+  }
+}