Adds flare command for approaching target at distance

src/main/java/frc/robot/RobotContainer.java

@@ -5,6 +5,9 @@
 package frc.robot;
 
 import com.ctre.phoenix.sensors.PigeonIMU;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -14,6 +17,8 @@
 import frc.robot.commands.auto.DoNothingCommand;
 import frc.robot.commands.delivery.DeliveryOverrideCommand;
 import frc.robot.commands.swerve.SwerveDriveCommand;
+import frc.robot.commands.teleop.DistanceToTargetCommand;
+import frc.robot.commands.teleop.FlareCommand;
 import frc.robot.nerdyfiles.oi.NerdyOperatorStation;
 import frc.robot.commands.shooter.RunKicker;
 import frc.robot.commands.shooter.StartShooter;
@@ -25,15 +30,15 @@ public class RobotContainer {
   private final NerdyOperatorStation operatorStation = new NerdyOperatorStation(2);
 
   private final PigeonIMU pigeon = new PigeonIMU(0);
-  private final PixyCam pixyCam = new PixyCam();
+  // private final PixyCam pixyCam = new PixyCam();
 
   // private final Climber climber = new Climber();
-  private final Intake intake = new Intake();
+  // private final Intake intake = new Intake();
   private final Shooter shooter = new Shooter();
   private final Kicker kicker = new Kicker();
-  private final Vision vision = new Vision();
+  // private final Vision vision = new Vision();
   private final AutoDrive autoDrive = new AutoDrive();
-  private final Delivery delivery = new Delivery();
+  // private final Delivery delivery = new Delivery();
   private final Drivetrain drivetrain = new Drivetrain(pigeon);
   private final Heading heading = new Heading(drivetrain::getGyroscopeRotation);
 
@@ -53,13 +58,19 @@ public RobotContainer() {
 
   public void resetRobot() {
     pigeon.setYaw(0, 250);
+    drivetrain.resetPosition(new Pose2d(
+      Constants.Auto.kBall2.toFieldCoordinate(),
+      drivetrain.getGyroscopeRotation()
+    ));
   }
 
   private void configureButtonBindings() {
     /** Driver Controller */
     // Note: Left X + Y axis, Right X axis, and Left Bumper are used by SwerveDriveCommand
     JoystickButton driverX = new JoystickButton(driverController, XboxController.Button.kX.value);
     driverX.whenPressed(heading::enableMaintainHeading);
+    JoystickButton driverRightBumper = new JoystickButton(driverController, XboxController.Button.kRightBumper.value);
+    driverRightBumper.whileHeld(new FlareCommand(drivetrain::getTranslation, autoDrive, drivetrain, heading));
     JoystickButton driverA = new JoystickButton(driverController, XboxController.Button.kA.value);
     driverA.whileHeld(new RunKicker(kicker));
     JoystickButton driverB = new JoystickButton(driverController, XboxController.Button.kB.value);

src/main/java/frc/robot/commands/HeadingToTargetCommand.java

@@ -17,7 +17,7 @@ public class HeadingToTargetCommand extends CommandBase {
 
   private Translation2d targetMeters;
   private Supplier<Translation2d> robotTranslationSupplier;
-  private Heading heading;
+  protected Heading heading;
 
   public HeadingToTargetCommand(Supplier<Translation2d> robotTranslationSupplier, Heading heading) {
     this(Constants.kHub, robotTranslationSupplier, heading);

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

@@ -0,0 +1,111 @@
+package frc.robot.commands.teleop;
+
+import java.util.function.Supplier;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
+import frc.robot.Constants;
+import frc.robot.commands.HeadingToTargetCommand;
+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 HeadingToTargetCommand implements AutoDrivableCommand {
+
+  private double distanceMeters;
+  private AutoDrive autoDrive;
+
+  private ProfiledPIDController distanceController;
+  private double output = 0.0;
+
+  public DistanceToTargetCommand(double distanceMeters, Supplier<Translation2d> translationSupplier, AutoDrive autoDrive, Heading heading) {
+    this(Constants.kHub, distanceMeters, translationSupplier, autoDrive, heading);
+  }
+
+  public DistanceToTargetCommand(Translation2d targetMeters, double distanceMeters, Supplier<Translation2d> translationSupplier, AutoDrive autoDrive, Heading heading) {
+    super(targetMeters, translationSupplier, heading);
+
+    this.distanceMeters = distanceMeters;
+    this.autoDrive = autoDrive;
+
+    distanceController = new ProfiledPIDController(1.5, 0.0, 0.0,
+      // Speeds have been tuned for 143" move (Ball 2 -> 10in from Hub)
+      new TrapezoidProfile.Constraints(
+        Units.inchesToMeters(160),
+        Units.inchesToMeters(80)
+      )
+    );
+    // 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()
+    );
+  }
+
+  @Override
+  public void execute() {
+    super.execute();
+
+    PolarCoordinate robotCoordinate = getRobotCoordinate();
+    output = distanceController.calculate(
+      robotCoordinate.getRadiusMeters(),
+      distanceMeters
+    );
+
+    Logger.getInstance().recordOutput("Flare/output", output);
+    Logger.getInstance().recordOutput("Flare/Error", distanceController.getPositionError());
+    Logger.getInstance().recordOutput("Flare/Velocity Error", distanceController.getVelocityError());
+    Logger.getInstance().recordOutput("Flare/atTarget", distanceController.atGoal());
+
+    // 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();
+  }
+
+  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
+    );
+  }
+
+}

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

@@ -0,0 +1,27 @@
+package frc.robot.commands.teleop;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.Constants;
+import frc.robot.subsystems.AutoDrive;
+import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.Heading;
+
+public class FlareCommand extends SequentialCommandGroup {
+
+  public FlareCommand(Supplier<Translation2d> robotTranslationSupplier, AutoDrive autoDrive, Drivetrain drivetrain, Heading heading) {
+    this(Constants.kHub, robotTranslationSupplier, autoDrive, drivetrain, heading);
+  }
+
+  public FlareCommand(Translation2d targetMeters, Supplier<Translation2d> robotTranslationSupplier, AutoDrive autoDrive, Drivetrain drivetrain, Heading heading) {
+    addCommands(
+      new HeadingToTargetSequenceCommand(targetMeters, robotTranslationSupplier, heading).withTimeout(1),
+      new DistanceToTargetCommand(targetMeters, Units.inchesToMeters(10), robotTranslationSupplier, autoDrive, heading)
+    );
+  }
+
+}

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

@@ -0,0 +1,38 @@
+package frc.robot.commands.teleop;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import frc.robot.Constants;
+import frc.robot.commands.HeadingToTargetCommand;
+import frc.robot.subsystems.Heading;
+
+/**
+ * Same command as the HeadingToTargeCommand but we will finish
+ * the command once we achieve the desired heading. This is useful
+ * for when we want to sequence rotating our heading to the target
+ * *and then* moving.
+ */
+public class HeadingToTargetSequenceCommand extends HeadingToTargetCommand {
+
+  public HeadingToTargetSequenceCommand(Supplier<Translation2d> robotTranslationSupplier, Heading heading) {
+    this(Constants.kHub, robotTranslationSupplier, heading);
+  }
+
+  public HeadingToTargetSequenceCommand(Translation2d target, Supplier<Translation2d> robotTranslationSupplier, Heading heading) {
+    super(target, robotTranslationSupplier, heading);
+  }
+
+  @Override
+  public void initialize() {
+    super.initialize();
+
+    heading.enableMaintainHeading();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return heading.atMaintainHeading();
+  }
+
+}

src/main/java/frc/robot/subsystems/Heading.java

@@ -68,7 +68,6 @@ public Heading(Supplier<Rotation2d> gyroAngleSupplier) {
     rotationController.setTolerance(1.0);
   }
 
-
   @Override
   public void periodic() {
     if (maintainHeading != null) {