Auto-aim

src/main/java/frc/robot/Constants.java

@@ -13,6 +13,8 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.util.Units;
+
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
  * constants. This class should not be used for any other purpose. All constants should be declared
@@ -27,9 +29,10 @@ public final class Constants {
   public static final double SHOOTER_DELAY = 1;
   public static final double INTAKE_SHOOTER_VOLTS = -8.0;
   public static final double INTAKE_HOPPER_VOLTS = -4.0;
-
   public static final double TICKS_PER_REVOLUTION = 1440;
   public static final Mode currentMode = Mode.REAL;
+  public static final double FIELD_LENGTH = Units.inchesToMeters(651.223);
+  public static final double SPEAKER_Y = Units.inchesToMeters(219.277);
 
   public static enum Mode {
     /** Running on a real robot. */

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

@@ -145,6 +145,8 @@ private void configureButtonBindings() {
                 .andThen(new LaunchNote(shooter, hopper))
                 .handleInterrupt(() -> shooter.stop()));
     controller.b().whileTrue(new IntakeNote(shooter, hopper).handleInterrupt(() -> shooter.stop()));
+    controller.x().whileTrue(new TurnToSpeaker(drive));
+    controller.y().whileTrue(AutoBuilder.buildAuto("Reset_Odometry"));
   }
 
   /**

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

@@ -0,0 +1,102 @@
+package frc.robot.commands;
+
+import static frc.robot.Constants.Mode.SIM;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.subsystems.drive.Drive;
+import org.littletonrobotics.junction.Logger;
+
+public class TurnToSpeaker extends Command {
+  private final Drive drive;
+  private final PIDController controller;
+  private static double kP = 0;
+  private static double kI = 0;
+  private static double kD = 0;
+  private static double minVelocity = 0;
+  private static double toleranceDegrees = 0;
+  private DriverStation.Alliance alliance = null;
+
+  /** Creates a new TurnToAngle. Turns to the specified rotation. */
+  public TurnToSpeaker(Drive drive) {
+    addRequirements(drive);
+    this.drive = drive;
+
+    switch (Constants.currentMode) {
+      case REAL:
+        kP = 0.018;
+        kI = 0.0;
+        kD = 0.0;
+        minVelocity = 0.0;
+        toleranceDegrees = 1.0;
+        break;
+      default: // for SIM
+        kP = 0.1;
+        kI = 0.0;
+        kD = 0.001;
+        minVelocity = 0.0;
+        toleranceDegrees = 1.5;
+        break;
+    }
+
+    controller = new PIDController(kP, kI, kD, 0.02);
+    controller.setTolerance(toleranceDegrees);
+    controller.enableContinuousInput(-180, 180);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    controller.reset();
+
+    if (DriverStation.getAlliance().isPresent()) {
+      this.alliance = DriverStation.getAlliance().get();
+    }
+
+    if (alliance == DriverStation.Alliance.Red) {
+      controller.setSetpoint(
+          Math.toDegrees(
+                  Math.atan2(
+                      Constants.SPEAKER_Y - drive.getPose().getY(),
+                      Constants.FIELD_LENGTH - drive.getPose().getX()))
+              - (Constants.currentMode == SIM ? Drive.YAW_DISPLACEMENT : 0));
+    } else {
+      controller.setSetpoint(
+          Math.toDegrees(
+              Math.atan2(Constants.SPEAKER_Y - drive.getPose().getY(), -drive.getPose().getX())
+                  - (Constants.currentMode == SIM ? Drive.YAW_DISPLACEMENT : 0)));
+    }
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    Logger.recordOutput("TurnActive", true);
+    Logger.recordOutput("Turn X Goal", controller.getSetpoint());
+    // Update output speeds
+
+    double angularSpeed = controller.calculate(drive.getGyroYawDegrees());
+
+    if (Math.abs(angularSpeed) < minVelocity) {
+      angularSpeed = Math.copySign(minVelocity, angularSpeed);
+    }
+
+    Logger.recordOutput("Angular Speed", angularSpeed);
+    drive.driveVelocity(
+        -angularSpeed * Drive.TRACK_WIDTH / 2.0, angularSpeed * Drive.TRACK_WIDTH / 2.0);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    Logger.recordOutput("TurnActive", false);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return controller.atSetpoint();
+  }
+}