WIP

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,7 @@
 import frc.robot.commands.auto.Top3Ball;
 import frc.robot.commands.delivery.DeliveryOverrideCommand;
 import frc.robot.commands.swerve.SwerveDriveCommand;
+import frc.robot.commands.teleop.DistanceToTargetCommand;
 import frc.robot.nerdyfiles.oi.NerdyOperatorStation;
 import frc.robot.commands.shooter.RunKicker;
 import frc.robot.commands.shooter.StartShooter;
@@ -53,24 +57,33 @@ public RobotContainer() {
 
   public void resetRobot() {
     pigeon.setYaw(0, 250);
+    drivetrain.resetPosition(new Pose2d(
+      Constants.Auto.startTopPosition.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 DistanceToTargetCommand(Units.inchesToMeters(130), () -> drivetrain.getPose().getTranslation(), drivetrain::getChassisSpeeds, autoDrive, heading));
+
+    /** Operator Controller * */
+    // Note: Left X is used by DeliveryOverrideCommand
     JoystickButton operatorA = new JoystickButton(operatorController, XboxController.Button.kA.value);
     operatorA.whenHeld(new StartShooter(shooter));
     JoystickButton operatorB = new JoystickButton(operatorController, XboxController.Button.kB.value);
     operatorB.whenHeld(new RunKicker(kicker));
-    JoystickButton operatorX = new JoystickButton(operatorController, XboxController.Button.kX.value);
-    JoystickButton driverX = new JoystickButton(driverController, XboxController.Button.kX.value);
-    driverX.whenPressed(heading::enableMaintainHeading);
-
-    // JoystickButton operatorLeftBumper = new JoystickButton(operatorController, XboxController.Button.kLeftBumper.value);
+    // JoystickButton operatorX = new JoystickButton(operatorController, XboxController.Button.kX.value);
+    // operatorX.whileHeld(new DeliveryOverrideCommand(operatorController, delivery));
     JoystickButton operatorRightBumper = new JoystickButton(operatorController, XboxController.Button.kRightBumper.value);
     operatorRightBumper.whenPressed(intake::start, intake);
     operatorRightBumper.whenReleased(intake::stop, intake);
 
     operatorStation.blueSwitch.whileHeld(new DeliveryOverrideCommand(operatorController, delivery));
-    //operatorX.whileHeld(new DeliveryOverrideCommand(operatorController, delivery));
   }
 
   public Command getAutonomousCommand() {

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

@@ -4,13 +4,12 @@
 
 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.HeadingToTargetCommand;
 import frc.robot.commands.interfaces.AutoDrivableCommand;
 import frc.robot.coordinates.PolarCoordinate;
 import frc.robot.subsystems.AutoDrive;
@@ -21,38 +20,38 @@
  * distance away from the point. Will attempt to face target while driving towards
  * target.
  */
-public class DistanceToTargetCommand extends OdometryHeadingCommand implements AutoDrivableCommand {
+public class DistanceToTargetCommand extends HeadingToTargetCommand implements AutoDrivableCommand {
 
   private Translation2d targetMeters;
   private double distanceMeters;
-  private Supplier<Pose2d> poseSupplier;
+  private Supplier<Translation2d> translationSupplier;
   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(double distanceMeters, Supplier<Translation2d> translationSupplier, Supplier<ChassisSpeeds> chassisSpeedsProvider, AutoDrive autoDrive, Heading heading) {
+    this(Constants.kHub, distanceMeters, translationSupplier, chassisSpeedsProvider, autoDrive, heading);
   }
 
-  public DistanceToTargetCommand(Translation2d targetMeters, double distanceMeters, Supplier<Pose2d> poseSupplier, Supplier<ChassisSpeeds> chassisSpeedsProvider, AutoDrive autoDrive, Heading heading) {
-    super(targetMeters, poseSupplier, heading);
+  public DistanceToTargetCommand(Translation2d targetMeters, double distanceMeters, Supplier<Translation2d> translationSupplier, Supplier<ChassisSpeeds> chassisSpeedsProvider, AutoDrive autoDrive, Heading heading) {
+    super(targetMeters, translationSupplier, heading);
 
     this.targetMeters = targetMeters;
     this.distanceMeters = distanceMeters;
-    this.poseSupplier = poseSupplier;
+    this.translationSupplier = translationSupplier;
     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,
+    final double kMaxSpeed = 0.5;
+    final double maxVelocityMetersPerSecond = Constants.Swerve.MAX_VELOCITY_METERS_PER_SECOND * kMaxSpeed;
+    distanceController = new ProfiledPIDController(2, 0.0, 0.0,
       // Use max speed above. Come up to max speed in one second
       new TrapezoidProfile.Constraints(
-        maxVelocityMetersPerSecond,
-        maxVelocityMetersPerSecond 
+        Units.inchesToMeters(120),
+        Units.inchesToMeters(120) / 2
       )
     );
     // Tolerance for our distance is within ~2 inches (should be Good Enough™️)
@@ -67,13 +66,48 @@ public void initialize() {
 
     autoDrive.setDelegate(this);
 
+    /**
+     * Calculate our current velocity relative to the target-
+     *
+     * 1) Find the robot's angle from the target. Should be a maximum of 45 degrees,
+     * as anything over 45 can be represented as an angle from a different angle.
+     * Ex: A 65 degree angle from a 0 to the target is a 25 degree angle from 90 to
+     * the target. Moving more vertical than horizontal in this case.
+     *
+     * 2) Find the modified vx or vy based on our new reference angle. Ex: A 25
+     * degree angle won't use our full vy velocity - we're not moving as fast away
+     * from the target on our y axis as on our x axis. Find the new side of the
+     * triangle. Will be less than that vy component in our example.
+     *
+     * 3) Find the new velocity based on our modified vx and vy
+     * v² = vx² + vy²
+     */
+    /*
+    PolarCoordinate robotCoordinate = getRobotCoordinate();
+    // Angle from point is the theta of our polar coordinate
+    Rotation2d theta = robotCoordinate.getTheta();
+    // Convert to some relative rotation - should help us with quadrants
+    double degrees = Utilities.convertRotationToRelativeRotation(theta).getDegrees();
+
+    boolean flipVy = false;
+    if (degrees > 45) {
+      degrees = degrees % 45;
+      flipVy = true;
+    }
+    */
+
     // 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
     );
+    */
+    distanceController.reset(
+      robotCoordinate.getRadiusMeters()
+    );
   }
 
   @Override
@@ -105,7 +139,7 @@ public void end(boolean interrupted) {
   private PolarCoordinate getRobotCoordinate() {
     // Find our robot polar coordinate relative to the target
     return PolarCoordinate.fromFieldCoordinate(
-      poseSupplier.get().getTranslation(),
+      translationSupplier.get(),
       targetMeters
     );
   }

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

@@ -168,6 +168,10 @@ public Rotation2d getGyroscopeRotation() {
     return Rotation2d.fromDegrees(pigeon.getYaw());
   }
 
+  public ChassisSpeeds getChassisSpeeds() {
+    return chassisSpeeds;
+  }
+
   public void drive(ChassisSpeeds chassisSpeeds) {
     this.chassisSpeeds = chassisSpeeds;
   }