Start Auto Aiming

src/main/java/frc/robot/Robot.java

@@ -13,7 +13,6 @@
 
 package frc.robot;
 
-import com.ctre.phoenix6.SignalLogger;
 import edu.wpi.first.hal.AllianceStationID;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Threads;
@@ -133,9 +132,6 @@ public void robotInit() {
     // Instantiate our RobotContainer. This will perform all our button bindings,
     // and put our autonomous chooser on the dashboard.
     robotContainer = new RobotContainer();
-
-    // Signal Logger
-    SignalLogger.setPath("/U");
   }
 
   /** This function is called periodically during all modes. */

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

@@ -192,6 +192,10 @@ private void configureButtonBindings() {
                                 FieldConstants.ampCenter.getY() - 0.6,
                                 new Rotation2d(Math.PI / 2.0))))))
         .onFalse(Commands.runOnce(drive::clearAutoAlignGoal));
+    controller
+        .x()
+        .onTrue(Commands.runOnce(drive::setAutoAimGoal))
+        .onFalse(Commands.runOnce(drive::clearAutoAimGoal));
     controller
         .b()
         .onTrue(

src/main/java/frc/robot/RobotState.java

@@ -11,6 +11,7 @@
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import frc.robot.subsystems.drive.DriveConstants;
+import frc.robot.util.AllianceFlipUtil;
 import java.util.NoSuchElementException;
 import org.littletonrobotics.junction.AutoLogOutput;
 
@@ -21,6 +22,9 @@ public record OdometryObservation(
 
   public record VisionObservation(Pose2d visionPose, double timestamp, Matrix<N3, N1> stdDevs) {}
 
+  public record AimingParameters(
+      Rotation2d driveHeading, double effectiveDistance, double radialFF) {}
+
   private static final double poseBufferSizeSeconds = 2.0;
 
   private static RobotState instance;
@@ -130,6 +134,31 @@ public void addVelocityData(Twist2d robotVelocity) {
     this.robotVelocity = robotVelocity;
   }
 
+  public AimingParameters getAimingParameters() {
+    Pose2d robot = getEstimatedPose();
+    Twist2d fieldVelocity = fieldVelocity();
+
+    Translation2d originToGoal =
+        AllianceFlipUtil.apply(FieldConstants.Speaker.centerSpeakerOpening.getTranslation());
+    Translation2d originToRobot = robot.getTranslation();
+
+    // Get robot to goal angle but limit to reasonable range
+    Rotation2d robotToGoalAngle = originToGoal.minus(originToGoal).getAngle();
+    // Subtract goal to robot angle from field velocity
+    Translation2d tangentialVelocity =
+        new Translation2d(fieldVelocity.dx, fieldVelocity.dy)
+            .rotateBy(robotToGoalAngle.unaryMinus());
+    // Subtract tangential velocity from goal to get virtual goal
+    Translation2d originToVirtualGoal = originToGoal.plus(tangentialVelocity.unaryMinus());
+
+    // Angle to virtual goal
+    Rotation2d driveHeading = originToVirtualGoal.minus(originToRobot).getAngle();
+    // Distance to virtual goal
+    double effectiveDistance = originToRobot.getDistance(originToVirtualGoal);
+    double radialFF = -tangentialVelocity.getX();
+    return new AimingParameters(driveHeading, effectiveDistance, radialFF);
+  }
+
   /**
    * Reset estimated pose and odometry pose to pose <br>
    * Clear pose buffer

src/main/java/frc/robot/commands/auto/AutoCommands.java

@@ -1,15 +1,10 @@
 package frc.robot.commands.auto;
 
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import frc.robot.FieldConstants;
 import frc.robot.RobotState;
 import frc.robot.subsystems.superstructure.intake.Intake;
-import frc.robot.util.AllianceFlipUtil;
-import frc.robot.util.shooting.ShotCalculator;
 import java.util.function.Supplier;
 
 public class AutoCommands {

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -22,6 +22,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.RobotState;
+import frc.robot.subsystems.drive.controllers.AutoAimController;
 import frc.robot.subsystems.drive.controllers.AutoAlignController;
 import frc.robot.subsystems.drive.controllers.TeleopDriveController;
 import frc.robot.subsystems.drive.controllers.TrajectoryController;
@@ -100,8 +101,9 @@ public static class OdometryTimestampInputs {
   private SwerveSetpointGenerator setpointGenerator;
 
   private final TeleopDriveController teleopDriveController;
-  private TrajectoryController trajectoryController;
-  private AutoAlignController autoAlignController;
+  private TrajectoryController trajectoryController = null;
+  private AutoAlignController autoAlignController = null;
+  private AutoAimController autoAimController = null;
 
   public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) {
     System.out.println("[Init] Creating Drive");
@@ -205,6 +207,10 @@ public void periodic() {
       case TELEOP -> {
         // Plain teleop drive
         desiredSpeeds = teleopSpeeds;
+        // Add auto aim if present
+        if (autoAimController != null) {
+          desiredSpeeds.omegaRadiansPerSecond = autoAimController.update();
+        }
       }
       case TRAJECTORY -> {
         // Run trajectory
@@ -240,7 +246,6 @@ public void periodic() {
           SwerveModuleState.optimize(currentSetpoint.moduleStates()[i], modules[i].getAngle());
       modules[i].runSetpoint(optimizedSetpointStates[i]);
     }
-
     // Log chassis speeds and swerve states
     Logger.recordOutput(
         "Drive/SwerveStates/Desired(b4 Poofs)",
@@ -298,6 +303,21 @@ public boolean isAutoAlignGoalCompleted() {
     return autoAlignController != null && autoAlignController.atGoal();
   }
 
+  /** Enable auto aiming on drive */
+  public void setAutoAimGoal() {
+    autoAimController = new AutoAimController();
+  }
+
+  /** Disable auto aiming on drive */
+  public void clearAutoAimGoal() {
+    autoAimController = null;
+  }
+
+  /** Returns true if robot is aimed at speaker */
+  public boolean isAutoAimGoalCompleted() {
+    return autoAimController != null && autoAimController.atSetpoint();
+  }
+
   /** Runs forwards at the commanded voltage. */
   public void runCharacterizationVolts(double volts) {
     currentDriveMode = DriveMode.CHARACTERIZATION;

src/main/java/frc/robot/subsystems/drive/DriveConstants.java

@@ -171,7 +171,7 @@ public static class KrakenDriveConstants {
   // Swerve Heading Control
   public static HeadingControllerConstants headingControllerConstants =
       switch (Constants.getRobot()) {
-        default -> new HeadingControllerConstants(3.0, 0.0);
+        default -> new HeadingControllerConstants(5.0, 0.0);
       };
 
   public record DriveConfig(
@@ -194,20 +194,20 @@ public record ModuleConfig(
       boolean turnMotorInverted) {}
 
   public record ModuleConstants(
-      double ffKs,
-      double ffKv,
-      double driveKp,
-      double driveKd,
-      double turnKp,
-      double turnKd,
+      double ffkS,
+      double ffkV,
+      double drivekP,
+      double drivekD,
+      double turnkP,
+      double turnkD,
       double driveReduction,
       double turnReduction) {}
 
   public record TrajectoryConstants(
-      double linearKp,
-      double linearKd,
-      double thetaKp,
-      double thetaKd,
+      double linearkP,
+      double linearkD,
+      double thetakP,
+      double thetakD,
       double linearTolerance,
       double thetaTolerance,
       double goalLinearTolerance,
@@ -216,18 +216,18 @@ public record TrajectoryConstants(
       double angularVelocityTolerance) {}
 
   public record AutoAlignConstants(
-      double linearKp,
-      double linearKd,
-      double thetaKp,
-      double thetaKd,
+      double linearkP,
+      double linearkD,
+      double thetakP,
+      double thetakD,
       double linearTolerance,
       double thetaTolerance,
       double maxLinearVelocity,
       double maxLinearAcceleration,
       double maxAngularVelocity,
       double maxAngularAcceleration) {}
 
-  public record HeadingControllerConstants(double Kp, double Kd) {}
+  public record HeadingControllerConstants(double kP, double kD) {}
 
   private enum Mk4iReductions {
     L2((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)),

src/main/java/frc/robot/subsystems/drive/Module.java

@@ -11,31 +11,31 @@
 
 public class Module {
   private static final LoggedTunableNumber driveKp =
-      new LoggedTunableNumber("Drive/Module/DriveKp", DriveConstants.moduleConstants.driveKp());
+      new LoggedTunableNumber("Drive/Module/DriveKp", DriveConstants.moduleConstants.drivekP());
   private static final LoggedTunableNumber driveKd =
-      new LoggedTunableNumber("Drive/Module/DriveKd", DriveConstants.moduleConstants.driveKd());
+      new LoggedTunableNumber("Drive/Module/DriveKd", DriveConstants.moduleConstants.drivekD());
   private static final LoggedTunableNumber driveKs =
-      new LoggedTunableNumber("Drive/Module/DriveKs", DriveConstants.moduleConstants.ffKs());
+      new LoggedTunableNumber("Drive/Module/DriveKs", DriveConstants.moduleConstants.ffkS());
   private static final LoggedTunableNumber driveKv =
-      new LoggedTunableNumber("Drive/Module/DriveKv", DriveConstants.moduleConstants.ffKv());
+      new LoggedTunableNumber("Drive/Module/DriveKv", DriveConstants.moduleConstants.ffkV());
   private static final LoggedTunableNumber turnKp =
-      new LoggedTunableNumber("Drive/Module/TurnKp", DriveConstants.moduleConstants.turnKp());
+      new LoggedTunableNumber("Drive/Module/TurnKp", DriveConstants.moduleConstants.turnkP());
   private static final LoggedTunableNumber turnKd =
-      new LoggedTunableNumber("Drive/Module/TurnKd", DriveConstants.moduleConstants.turnKd());
+      new LoggedTunableNumber("Drive/Module/TurnKd", DriveConstants.moduleConstants.turnkD());
 
   private final int index;
   private final ModuleIO io;
   private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
 
   private final PIDController driveController =
       new PIDController(
-          DriveConstants.moduleConstants.driveKp(), 0.0, DriveConstants.moduleConstants.driveKd());
+          DriveConstants.moduleConstants.drivekP(), 0.0, DriveConstants.moduleConstants.drivekD());
   private final PIDController turnController =
       new PIDController(
-          DriveConstants.moduleConstants.turnKp(), 0.0, DriveConstants.moduleConstants.turnKd());
+          DriveConstants.moduleConstants.turnkP(), 0.0, DriveConstants.moduleConstants.turnkD());
   private SimpleMotorFeedforward driveFF =
       new SimpleMotorFeedforward(
-          DriveConstants.moduleConstants.ffKs(), DriveConstants.moduleConstants.ffKv(), 0.0);
+          DriveConstants.moduleConstants.ffkS(), DriveConstants.moduleConstants.ffkV(), 0.0);
 
   @Getter private SwerveModuleState setpointState = new SwerveModuleState();
 

src/main/java/frc/robot/subsystems/drive/controllers/AutoAimController.java

@@ -0,0 +1,46 @@
+package frc.robot.subsystems.drive.controllers;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.util.Units;
+import frc.robot.RobotState;
+import frc.robot.subsystems.drive.DriveConstants;
+import frc.robot.util.LoggedTunableNumber;
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+public class AutoAimController {
+  private static final LoggedTunableNumber headingkP =
+      new LoggedTunableNumber("AutoAim/kP", DriveConstants.headingControllerConstants.kP());
+  private static final LoggedTunableNumber headingkD =
+      new LoggedTunableNumber("AutoAim/kD", DriveConstants.headingControllerConstants.kD());
+  private static final LoggedTunableNumber tolerance =
+      new LoggedTunableNumber("AutoAim/ToleranceDegrees", 4.0);
+
+  private PIDController headingController;
+
+  public AutoAimController() {
+    headingController = new PIDController(0, 0, 0, 0.02);
+    headingController.enableContinuousInput(-Math.PI, Math.PI);
+  }
+
+  /** Returns the rotation rate to turn to aim at speaker */
+  public double update() {
+    headingController.setPID(headingkP.get(), 0, headingkD.get());
+    headingController.setTolerance(Units.degreesToRadians(tolerance.get()));
+
+    var aimingParams = RobotState.getInstance().getAimingParameters();
+    double output =
+        headingController.calculate(
+            RobotState.getInstance().getEstimatedPose().getRotation().getRadians(),
+            aimingParams.driveHeading().getRadians());
+
+    Logger.recordOutput("AutoAim/HeadingError", headingController.getPositionError());
+    return output;
+  }
+
+  /** Returns true if within tolerance of aiming at speaker */
+  @AutoLogOutput(key = "AutoAim/AtSetpoint")
+  public boolean atSetpoint() {
+    return headingController.atSetpoint();
+  }
+}

src/main/java/frc/robot/subsystems/drive/controllers/AutoAlignController.java

@@ -16,13 +16,13 @@
 
 public class AutoAlignController {
   private static LoggedTunableNumber linearkP =
-      new LoggedTunableNumber("AutoAlign/drivekP", DriveConstants.autoAlignConstants.linearKp());
+      new LoggedTunableNumber("AutoAlign/drivekP", DriveConstants.autoAlignConstants.linearkP());
   private static LoggedTunableNumber linearkD =
-      new LoggedTunableNumber("AutoAlign/drivekD", DriveConstants.autoAlignConstants.linearKd());
+      new LoggedTunableNumber("AutoAlign/drivekD", DriveConstants.autoAlignConstants.linearkD());
   private static LoggedTunableNumber thetakP =
-      new LoggedTunableNumber("AutoAlign/thetakP", DriveConstants.autoAlignConstants.thetaKp());
+      new LoggedTunableNumber("AutoAlign/thetakP", DriveConstants.autoAlignConstants.thetakP());
   private static LoggedTunableNumber thetakD =
-      new LoggedTunableNumber("AutoAlign/thetakD", DriveConstants.autoAlignConstants.thetaKd());
+      new LoggedTunableNumber("AutoAlign/thetakD", DriveConstants.autoAlignConstants.thetakD());
   private static LoggedTunableNumber linearTolerance =
       new LoggedTunableNumber(
           "AutoAlign/controllerLinearTolerance",
@@ -105,13 +105,13 @@ public ChassisSpeeds update() {
     LoggedTunableNumber.ifChanged(
         hashCode(),
         () -> linearController.setPID(linearkP.get(), 0, linearkD.get()),
-            linearkP,
-            linearkD);
+        linearkP,
+        linearkD);
     LoggedTunableNumber.ifChanged(
         hashCode(),
         () -> thetaController.setPID(thetakP.get(), 0, thetakD.get()),
-            thetakP,
-            thetakD);
+        thetakP,
+        thetakD);
     LoggedTunableNumber.ifChanged(
         hashCode(), () -> linearController.setTolerance(linearTolerance.get()), linearTolerance);
     LoggedTunableNumber.ifChanged(

src/main/java/frc/robot/subsystems/drive/controllers/TrajectoryController.java

@@ -19,13 +19,13 @@
 
 public class TrajectoryController {
   private static LoggedTunableNumber trajectoryLinearkP =
-      new LoggedTunableNumber("Trajectory/linearkP", trajectoryConstants.linearKp());
+      new LoggedTunableNumber("Trajectory/linearkP", trajectoryConstants.linearkP());
   private static LoggedTunableNumber trajectoryLinearkD =
-      new LoggedTunableNumber("Trajectory/linearkD", trajectoryConstants.linearKd());
+      new LoggedTunableNumber("Trajectory/linearkD", trajectoryConstants.linearkD());
   private static LoggedTunableNumber trajectoryThetakP =
-      new LoggedTunableNumber("Trajectory/thetakP", trajectoryConstants.thetaKp());
+      new LoggedTunableNumber("Trajectory/thetakP", trajectoryConstants.thetakP());
   private static LoggedTunableNumber trajectoryThetakD =
-      new LoggedTunableNumber("Trajectory/thetakD", trajectoryConstants.thetaKd());
+      new LoggedTunableNumber("Trajectory/thetakD", trajectoryConstants.thetakD());
   private static LoggedTunableNumber trajectoryLinearTolerance =
       new LoggedTunableNumber(
           "Trajectory/controllerLinearTolerance", trajectoryConstants.linearTolerance());
@@ -74,10 +74,10 @@ public ChassisSpeeds update() {
     LoggedTunableNumber.ifChanged(
         hashCode(),
         pid -> controller.setPID(pid[0], pid[1], pid[2], pid[3]),
-            trajectoryLinearkP,
-            trajectoryLinearkD,
-            trajectoryThetakP,
-            trajectoryThetakP);
+        trajectoryLinearkP,
+        trajectoryLinearkD,
+        trajectoryThetakP,
+        trajectoryThetakP);
     // Tolerances
     LoggedTunableNumber.ifChanged(
         hashCode(),

src/main/java/frc/robot/subsystems/superstructure/Arm/Arm.java

@@ -21,11 +21,11 @@
 
 public class Arm extends SubsystemBase {
   private static final LoggedTunableNumber kP =
-      new LoggedTunableNumber("Arm/Kp", controllerConstants.kP());
+      new LoggedTunableNumber("Arm/kP", controllerConstants.kP());
   private static final LoggedTunableNumber kI =
       new LoggedTunableNumber("Arm/Ki", controllerConstants.kI());
   private static final LoggedTunableNumber kD =
-      new LoggedTunableNumber("Arm/Kd", controllerConstants.kD());
+      new LoggedTunableNumber("Arm/kD", controllerConstants.kD());
   private static final LoggedTunableNumber kS =
       new LoggedTunableNumber("Arm/Ks", controllerConstants.ffkS());
   private static final LoggedTunableNumber kV =