Speaker targeting

src/main/java/org/team1540/robot2024/Constants.java

@@ -1,5 +1,6 @@
 package org.team1540.robot2024;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
@@ -232,6 +233,16 @@ public static class Tramp {
         public static final int MOTOR_ID = -1; //TODO: Configure this later
     }
 
+    public static class Targeting {
+        // TODO: tune these
+        public static final double ROT_KP = 1.18;
+        public static final double ROT_KI = 0.0;
+        public static final double ROT_KD = 0.0;
+
+        public static final Pose2d SPEAKER_POSE =
+                new Pose2d(Units.inchesToMeters(8.861), Units.inchesToMeters(218), new Rotation2d());
+    }
+
     public static boolean isTuningMode() {
         return tuningMode && !DriverStation.isFMSAttached();
     }

src/main/java/org/team1540/robot2024/RobotContainer.java

@@ -9,15 +9,14 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
-import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 import org.team1540.robot2024.Constants.Elevator.ElevatorState;
 import org.team1540.robot2024.commands.FeedForwardCharacterization;
+import org.team1540.robot2024.commands.DriveWithSpeakerTargetingCommand;
 import org.team1540.robot2024.commands.SwerveDriveCommand;
 import org.team1540.robot2024.commands.elevator.ElevatorManualCommand;
 import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand;
 import org.team1540.robot2024.commands.indexer.IntakeCommand;
 import org.team1540.robot2024.commands.shooter.ShootSequence;
-import org.team1540.robot2024.commands.shooter.TuneShooterCommand;
 import org.team1540.robot2024.subsystems.drive.*;
 import org.team1540.robot2024.subsystems.elevator.Elevator;
 import org.team1540.robot2024.subsystems.elevator.ElevatorIO;
@@ -176,6 +175,7 @@ private void configureButtonBindings() {
         indexer.setDefaultCommand(new IntakeCommand(indexer, tramp));
 
         driver.x().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain));
+        driver.y().toggleOnFalse(new DriveWithSpeakerTargetingCommand(drivetrain, driver));
         driver.b().onTrue(
                 Commands.runOnce(
                         () -> drivetrain.setPose(new Pose2d(drivetrain.getPose().getTranslation(), new Rotation2d())),

src/main/java/org/team1540/robot2024/commands/DriveWithSpeakerTargetingCommand.java

@@ -0,0 +1,72 @@
+package org.team1540.robot2024.commands;
+
+import com.pathplanner.lib.util.GeometryUtil;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import org.littletonrobotics.junction.Logger;
+import org.team1540.robot2024.subsystems.drive.Drivetrain;
+import org.team1540.robot2024.util.LoggedTunableNumber;
+
+import static org.team1540.robot2024.Constants.Targeting.*;
+
+public class DriveWithSpeakerTargetingCommand extends Command {
+    private final Drivetrain drivetrain;
+    private final CommandXboxController controller;
+
+    private final SlewRateLimiter xLimiter = new SlewRateLimiter(2);
+    private final SlewRateLimiter yLimiter = new SlewRateLimiter(2);
+    private final PIDController rotController = new PIDController(ROT_KP, ROT_KI, ROT_KD);
+
+    private final LoggedTunableNumber kP = new LoggedTunableNumber("Targeting/ROT_KP", ROT_KP);
+    private final LoggedTunableNumber kI = new LoggedTunableNumber("Targeting/ROT_KI", ROT_KI);
+    private final LoggedTunableNumber kD = new LoggedTunableNumber("Targeting/ROT_KD", ROT_KD);
+
+    private boolean isFlipped;
+    private Pose2d speakerPose;
+
+    public DriveWithSpeakerTargetingCommand(Drivetrain drivetrain, CommandXboxController controller) {
+        this.drivetrain = drivetrain;
+        this.controller = controller;
+        rotController.enableContinuousInput(-Math.PI, Math.PI);
+        addRequirements(drivetrain);
+    }
+
+    @Override
+    public void initialize() {
+        rotController.reset();
+
+        isFlipped =
+                DriverStation.getAlliance().isPresent()
+                        && DriverStation.getAlliance().get() == DriverStation.Alliance.Red;
+        speakerPose = isFlipped ? GeometryUtil.flipFieldPose(SPEAKER_POSE) : SPEAKER_POSE;
+    }
+
+    @Override
+    public void execute() {
+        if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode()) || kD.hasChanged(hashCode())) {
+            rotController.setPID(kP.get(), kI.get(), kD.get());
+        }
+
+        Rotation2d targetRot =
+                drivetrain.getPose().minus(speakerPose).getTranslation().getAngle()
+                        .rotateBy(isFlipped ? Rotation2d.fromDegrees(0) : Rotation2d.fromDegrees(180));
+        Logger.recordOutput("Targeting/setpointPose", new Pose2d(drivetrain.getPose().getTranslation(), targetRot));
+        Logger.recordOutput("Targeting/speakerPose", speakerPose);
+
+        double xPercent = MathUtil.applyDeadband(xLimiter.calculate(-controller.getLeftY()), 0.1);
+        double yPercent = MathUtil.applyDeadband(yLimiter.calculate(-controller.getLeftX()), 0.1);
+        double rotPercent = rotController.calculate(drivetrain.getRotation().getRadians(), targetRot.getRadians());
+        drivetrain.drivePercent(xPercent, yPercent, rotPercent, isFlipped);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        drivetrain.stop();
+    }
+}

src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java

@@ -2,11 +2,6 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.SlewRateLimiter;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -30,41 +25,18 @@ public SwerveDriveCommand(Drivetrain drivetrain, CommandXboxController controlle
 
     @Override
     public void initialize() {
-        xLimiter.reset(0);
-        yLimiter.reset(0);
-        rotLimiter.reset(0);
         isFlipped =
                 DriverStation.getAlliance().isPresent()
                         && DriverStation.getAlliance().get() == DriverStation.Alliance.Red;
     }
 
     @Override
     public void execute() {
-        double xPercent = xLimiter.calculate(-controller.getLeftY());
-        double yPercent = yLimiter.calculate(-controller.getLeftX());
-        double rotPercent = rotLimiter.calculate(-controller.getRightX());
+        double xPercent = MathUtil.applyDeadband(xLimiter.calculate(-controller.getLeftY()), 0.1);
+        double yPercent = MathUtil.applyDeadband(yLimiter.calculate(-controller.getLeftX()), 0.1);
+        double rotPercent = MathUtil.applyDeadband(rotLimiter.calculate(-controller.getRightX()), 0.1);
 
-        // Apply deadband
-        double linearMagnitude = MathUtil.applyDeadband(Math.hypot(xPercent, yPercent), 0.1);
-        Rotation2d linearDirection = new Rotation2d(xPercent, yPercent);
-        double omega = MathUtil.applyDeadband(rotPercent, 0.1);
-
-        // Calculate new linear velocity
-        Translation2d linearVelocity =
-                new Pose2d(new Translation2d(), linearDirection)
-                        .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation();
-
-        // Convert to field relative speeds & send command
-        drivetrain.runVelocity(
-                ChassisSpeeds.fromFieldRelativeSpeeds(
-                        linearVelocity.getX() * drivetrain.getMaxLinearSpeedMetersPerSec(),
-                        linearVelocity.getY() * drivetrain.getMaxLinearSpeedMetersPerSec(),
-                        omega * drivetrain.getMaxAngularSpeedRadPerSec(),
-                        isFlipped
-                                ? drivetrain.getRotation().plus(Rotation2d.fromDegrees(180))
-                                : drivetrain.getRotation()
-                )
-        );
+        drivetrain.drivePercent(xPercent, yPercent, rotPercent, isFlipped);
     }
 
     @Override

src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
@@ -125,6 +126,27 @@ public void runVelocity(ChassisSpeeds speeds) {
         Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates);
     }
 
+    public void drivePercent(double xPercent, double yPercent, double rotPercent, boolean isFlipped) {
+        Rotation2d linearDirection = new Rotation2d(xPercent, yPercent);
+        double linearMagnitude = Math.hypot(xPercent, yPercent);
+
+        Translation2d linearVelocity =
+                new Pose2d(new Translation2d(), linearDirection)
+                        .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation();
+
+        // Convert to field relative
+        runVelocity(
+                ChassisSpeeds.fromFieldRelativeSpeeds(
+                        linearVelocity.getX() * getMaxLinearSpeedMetersPerSec(),
+                        linearVelocity.getY() * getMaxLinearSpeedMetersPerSec(),
+                        rotPercent * getMaxAngularSpeedRadPerSec(),
+                        isFlipped
+                                ? getRotation().plus(Rotation2d.fromDegrees(180))
+                                : getRotation()
+                )
+        );
+    }
+
     /**
      * Stops the drive.
      */