Two

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

@@ -8,6 +8,7 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.I2C;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -40,10 +41,13 @@ public class RobotContainer {
   private final AutoDrive autoDrive = new AutoDrive();
   // private final Delivery delivery = new Delivery();
   private final Drivetrain drivetrain = new Drivetrain(pigeon);
-  private final Heading heading = new Heading(drivetrain::getGyroscopeRotation);
+  private final Heading heading = new Heading(drivetrain::getGyroscopeRotation, drivetrain::getChassisSpeeds);
 
   private final SendableChooser<Command> autonChooser = new SendableChooser<>();
 
+  // private final ColorSensorREV c1 = new ColorSensorREV(I2C.Port.kOnboard);
+  private final ColorSensorTCS c2 = new ColorSensorTCS(I2C.Port.kMXP);
+
   public RobotContainer() {
     drivetrain.setDefaultCommand(new SwerveDriveCommand(driverController, autoDrive, heading, drivetrain));
     heading.setDefaultCommand(new HeadingToTargetCommand(() -> drivetrain.getPose().getTranslation(), heading));
@@ -59,7 +63,7 @@ public RobotContainer() {
   public void resetRobot() {
     pigeon.setYaw(0, 250);
     drivetrain.resetPosition(new Pose2d(
-      Constants.Auto.kBall2.toFieldCoordinate(),
+      Constants.Auto.kBallR2.toFieldCoordinate(),
       drivetrain.getGyroscopeRotation()
     ));
   }

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

@@ -5,6 +5,7 @@
 import org.littletonrobotics.junction.Logger;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
@@ -26,7 +27,9 @@ public class DistanceToTargetCommand extends HeadingToTargetCommand implements A
   private double distanceMeters;
   private AutoDrive autoDrive;
 
-  private ProfiledPIDController distanceController;
+  private TrapezoidProfile profile;
+  // private ProfiledPIDController distanceController;
+  private PIDController distanceController;
   private double output = 0.0;
 
   public DistanceToTargetCommand(double distanceMeters, Supplier<Translation2d> translationSupplier, AutoDrive autoDrive, Heading heading) {
@@ -39,13 +42,16 @@ public DistanceToTargetCommand(Translation2d targetMeters, double distanceMeters
     this.distanceMeters = distanceMeters;
     this.autoDrive = autoDrive;
 
+    distanceController = new PIDController(1.5, 0.0, 0.0);
+    /*
     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)
+        Units.inchesToMeters(50)
       )
     );
+    */
     // Tolerance for our distance is within ~2 inches (should be Good Enough™️)
     distanceController.setTolerance(Units.inchesToMeters(2));
 
@@ -58,11 +64,22 @@ public void initialize() {
 
     autoDrive.setDelegate(this);
 
-    // Set our initial setpoint for our profiled PID controllers
-    // to avoid a JUMP to their starting values on first run
+    // Set our initial setpoint for our profile to avoid
+    // a JUMP to their starting values on first run
     PolarCoordinate robotCoordinate = getRobotCoordinate();
-    distanceController.reset(
-      robotCoordinate.getRadiusMeters()
+    profile = new TrapezoidProfile(
+      new TrapezoidProfile.Constraints(
+        Units.inchesToMeters(160),
+        Units.inchesToMeters(60)
+      ),
+      new TrapezoidProfile.State(
+        distanceMeters,
+        0
+      ),
+      new TrapezoidProfile.State(
+        robotCoordinate.getRadiusMeters(),
+        0
+      )
     );
   }
 
@@ -71,15 +88,23 @@ public void execute() {
     super.execute();
 
     PolarCoordinate robotCoordinate = getRobotCoordinate();
+    var m_setpoint = profile.calculate(distanceController.getPeriod());
     output = distanceController.calculate(
       robotCoordinate.getRadiusMeters(),
-      distanceMeters
+      m_setpoint.position
     );
 
     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());
+    Logger.getInstance().recordOutput("Flare/Robot Distance", robotCoordinate.getRadiusMeters());
+    Logger.getInstance().recordOutput("Flare/Goal Position", distanceController.getGoal().position);
+    Logger.getInstance().recordOutput("Flare/Goal Velocity", distanceController.getGoal().velocity);
+    Logger.getInstance().recordOutput("Flare/Setpoint Position", distanceController.getSetpoint().position);
+    Logger.getInstance().recordOutput("Flare/Setpoint Velocity", distanceController.getSetpoint().velocity);
+    */
 
     // Clamp to some max speed (should be between [0.0, 1.0])
     final double maxSpeed = 1.0;

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

@@ -188,6 +188,10 @@ public void drive(ChassisSpeeds chassisSpeeds) {
     this.chassisSpeeds = chassisSpeeds;
   }
 
+  public ChassisSpeeds getChassisSpeeds() {
+    return chassisSpeeds;
+  }
+
   public void resetOdometry() {
     odometry.resetPosition(new Pose2d(0, 0, Rotation2d.fromDegrees(0)), Rotation2d.fromDegrees(0));
   }

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

@@ -7,6 +7,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.nerdyfiles.utilities.Utilities;
 
@@ -29,13 +30,16 @@ public class Heading extends SubsystemBase {
    * NOTE: Currently disabled by default as not to cause confusion when
    * we're testing things on the robot.
    */
-  private boolean enabled = false;
+  private boolean enabled = true;
 
   /**
    * Supplier to provide the gyro angle of the robot. Should come from
    * the gyro on the Drivetrain. Used to calculate our maintain heading calculation.
    */
   private Supplier<Rotation2d> gyroAngleSupplier;
+
+  private Supplier<ChassisSpeeds> chassisSpeedsSupplier;
+
   /**
    * The maintain heading is the heading we would like the robot to maintain.
    * Can be null if the robot should not maintain any specific heading.
@@ -52,16 +56,20 @@ public class Heading extends SubsystemBase {
   /**
    * PID used to converge the robot to the maintainHeading from it's current heading.
    */
-  private PIDController rotationController = new PIDController(0.01, 0.0, 0.0);
+  private PIDController rotationController = new PIDController(0.004, 0.0, 0.0);
+  private static double kFeedForward = 0.015;
+  private static double kNominalStationaryOutput = 0.03;
+  private static double kNominalMovingOutput = 0.02;
 
   /**
    * Heading subsystem to maintain a static heading of the robot.
    *
    * @param gyroAngleSupplier A Supplier to provide the gyro value of the robot.
    *                          Should come from the Pigeon.
    */
-  public Heading(Supplier<Rotation2d> gyroAngleSupplier) {
+  public Heading(Supplier<Rotation2d> gyroAngleSupplier, Supplier<ChassisSpeeds> chassisSpeedsSupplier) {
     this.gyroAngleSupplier = gyroAngleSupplier;
+    this.chassisSpeedsSupplier = chassisSpeedsSupplier;
 
     rotationController.enableContinuousInput(-180, 180);
     // +/- 1 degree position error tolerance
@@ -201,15 +209,21 @@ public double calculateRotation() {
       currentHeading.getDegrees(),
       maintainHeading.getDegrees()
     );
-    Logger.getInstance().recordOutput("Heading/Rotation Controller Output", output);
+    if (rotationController.atSetpoint()) {
+      return 0.0;
+    }
     // Clamp to some max speed (should be between [0.0, 1.0])
     final double maxSpeed = 0.3;
-    double clamedOutput = MathUtil.clamp(
+    double clampedOutput = MathUtil.clamp(
       output,
       -maxSpeed,
       maxSpeed
-    );
-    return clamedOutput;
+      );
+      ChassisSpeeds chassisSpeeds = chassisSpeedsSupplier.get();
+      double velocity = Math.hypot(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond);
+      double adjustedClampedOutput = Math.copySign(Math.max(Math.abs(clampedOutput), velocity > 0 ? kNominalMovingOutput : kNominalStationaryOutput), clampedOutput);
+      Logger.getInstance().recordOutput("Heading/Rotation Controller Output", adjustedClampedOutput);
+      return adjustedClampedOutput;
   }
 
   /**