Auto align and scoring

build.gradle

@@ -111,8 +111,8 @@ test {
 // AdvantageKit log replay from the command line. Set the
 // value to "true" to enable the sim GUI by default (this
 // is the standard WPILib behavior).
-wpi.sim.addGui().defaultEnabled = true
-// wpi.sim.addDriverstation()
+wpi.sim.addGui().defaultEnabled = false
+wpi.sim.addDriverstation().defaultEnabled = true
 
 // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
 // in order to make them all available at runtime. Also adding the manifest so WPILib

src/main/java/frc/robot/FieldConstants.java

@@ -0,0 +1,33 @@
+package frc.robot;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import java.util.ArrayList;
+
+/** Just a bunch of measurements of various parts of the field, measured from the blue origin. */
+public class FieldConstants {
+
+    public static final AprilTagFieldLayout tagLayout =
+            AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded);
+
+    public static final double fieldLength = tagLayout.getFieldLength();
+    public static final double fieldWidth = tagLayout.getFieldWidth();
+    public static final Translation2d fieldCenter = new Translation2d(fieldLength / 2, fieldWidth / 2);
+
+    public static class Reef {
+
+        // Starting at the A/B side, and rotating clockwise until the C/D side.
+        public static ArrayList<Pose2d> tagPositions = new ArrayList<Pose2d>(6);
+
+        static {
+            tagPositions.add(tagLayout.getTagPose(18).get().toPose2d());
+            tagPositions.add(tagLayout.getTagPose(19).get().toPose2d());
+            tagPositions.add(tagLayout.getTagPose(20).get().toPose2d());
+            tagPositions.add(tagLayout.getTagPose(21).get().toPose2d());
+            tagPositions.add(tagLayout.getTagPose(22).get().toPose2d());
+            tagPositions.add(tagLayout.getTagPose(17).get().toPose2d());
+        }
+    }
+}

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

@@ -13,26 +13,24 @@
 
 package frc.robot;
 
-import static edu.wpi.first.units.Units.*;
 import static frc.robot.subsystems.vision.VisionConstants.*;
 import static frc.robot.subsystems.vision.VisionConstants.robotToCamera1;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.commands.DriveCommands;
+import frc.robot.commands.SuperstructureCommands;
 import frc.robot.subsystems.drive.*;
 import frc.robot.subsystems.vision.*;
 import org.ironmaple.simulation.SimulatedArena;
 import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
-import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeCoralOnFly;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
@@ -53,6 +51,8 @@ public class RobotContainer {
     // Dashboard inputs
     private final LoggedDashboardChooser<Command> autoChooser;
 
+    private final boolean isSim;
+
     /** The container for the robot. Contains subsystems, OI devices, and commands. */
     public RobotContainer() {
         switch (Constants.currentMode) {
@@ -70,6 +70,7 @@ public RobotContainer() {
                         drive,
                         new VisionIOPhotonVision(VisionConstants.camera0Name, VisionConstants.robotToCamera0),
                         new VisionIOPhotonVision(VisionConstants.camera1Name, VisionConstants.robotToCamera1));
+                isSim = false;
 
                 break;
             case SIM:
@@ -94,6 +95,8 @@ public RobotContainer() {
                         new VisionIOPhotonVisionSim(
                                 camera1Name, robotToCamera1, driveSimulation::getSimulatedDriveTrainPose));
 
+                isSim = true;
+
                 break;
             default:
                 // Replayed robot, disable IO implementations
@@ -106,6 +109,8 @@ public RobotContainer() {
                         (pose) -> {});
                 vision = new Vision(drive, new VisionIO() {}, new VisionIO() {});
 
+                isSim = true;
+
                 break;
         }
 
@@ -154,30 +159,31 @@ private void configureButtonBindings() {
                         new Pose2d(drive.getPose().getTranslation(), new Rotation2d())); // zero gyro
         controller.start().onTrue(Commands.runOnce(resetGyro, drive).ignoringDisable(true));
 
-        // Example Coral Placement Code
-        // TODO: delete these code for your own project
-        if (Constants.currentMode == Constants.Mode.SIM) {
-            // L4 placement
-            controller.y().onTrue(Commands.runOnce(() -> SimulatedArena.getInstance()
-                    .addGamePieceProjectile(new ReefscapeCoralOnFly(
-                            driveSimulation.getSimulatedDriveTrainPose().getTranslation(),
-                            new Translation2d(0.4, 0),
-                            driveSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative(),
-                            driveSimulation.getSimulatedDriveTrainPose().getRotation(),
-                            Meters.of(2),
-                            MetersPerSecond.of(1.5),
-                            Degrees.of(-80)))));
-            // L3 placement
-            controller.b().onTrue(Commands.runOnce(() -> SimulatedArena.getInstance()
-                    .addGamePieceProjectile(new ReefscapeCoralOnFly(
-                            driveSimulation.getSimulatedDriveTrainPose().getTranslation(),
-                            new Translation2d(0.4, 0),
-                            driveSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative(),
-                            driveSimulation.getSimulatedDriveTrainPose().getRotation(),
-                            Meters.of(1.35),
-                            MetersPerSecond.of(1.5),
-                            Degrees.of(-60)))));
-        }
+        controller
+                .rightTrigger()
+                .whileTrue(isSim ? SuperstructureCommands.scoreRightL4Sim(drive, driveSimulation) : Commands.none());
+
+        controller
+                .rightBumper()
+                .whileTrue(isSim ? SuperstructureCommands.scoreRightL3Sim(drive, driveSimulation) : Commands.none());
+
+        controller
+                .leftTrigger()
+                .whileTrue(isSim ? SuperstructureCommands.scoreLeftL4Sim(drive, driveSimulation) : Commands.none());
+
+        controller
+                .leftBumper()
+                .whileTrue(isSim ? SuperstructureCommands.scoreLeftL3Sim(drive, driveSimulation) : Commands.none());
+
+        controller
+                .rightBumper()
+                .and(controller.rightTrigger())
+                .whileTrue(isSim ? SuperstructureCommands.scoreRightL2Sim(drive, driveSimulation) : Commands.none());
+
+        controller
+                .leftBumper()
+                .and(controller.leftTrigger())
+                .whileTrue(isSim ? SuperstructureCommands.scoreLeftL2Sim(drive, driveSimulation) : Commands.none());
     }
 
     /**

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

@@ -14,6 +14,7 @@
 package frc.robot.commands;
 
 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.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -28,10 +29,12 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.FieldConstants;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.DriveConstants;
 import java.text.DecimalFormat;
 import java.text.NumberFormat;
+import java.util.ArrayList;
 import java.util.LinkedList;
 import java.util.List;
 import java.util.function.DoubleSupplier;
@@ -69,9 +72,13 @@ public static Command joystickDrive(
             Drive drive, DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier omegaSupplier) {
         return Commands.run(
                 () -> {
+                    boolean isFlipped = DriverStation.getAlliance().isPresent()
+                            && DriverStation.getAlliance().get() == Alliance.Red;
+
                     // Get linear velocity
-                    Translation2d linearVelocity =
-                            getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble());
+                    Translation2d linearVelocity = getLinearVelocityFromJoysticks(
+                            xSupplier.getAsDouble() * (isFlipped ? -1 : 1),
+                            ySupplier.getAsDouble() * (isFlipped ? -1 : 1));
 
                     // Apply rotation deadband
                     double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND);
@@ -84,8 +91,7 @@ public static Command joystickDrive(
                             linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
                             linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
                             omega * drive.getMaxAngularSpeedRadPerSec());
-                    boolean isFlipped = DriverStation.getAlliance().isPresent()
-                            && DriverStation.getAlliance().get() == Alliance.Red;
+
                     speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
                             speeds,
                             isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation());
@@ -138,6 +144,82 @@ public static Command joystickDriveAtAngle(
                 .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians()));
     }
 
+    public static Command alignToReef(Drive drive, boolean alignLeft) {
+        ArrayList<Pose2d> reefTagPositions = FieldConstants.Reef.tagPositions;
+
+        boolean isFlipped = DriverStation.getAlliance().isPresent()
+                && DriverStation.getAlliance().get() == Alliance.Red;
+
+        if (isFlipped) {
+            for (int i = 0; i < 6; i++) {
+                reefTagPositions.set(
+                        i, reefTagPositions.get(i).rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg));
+            }
+        }
+
+        PIDController xController = new PIDController(4.25, 0, 0.2);
+        PIDController yController = new PIDController(4.25, 0, 0.2);
+        PIDController thetaController = new PIDController(1.8, 0, 0.1);
+        thetaController.enableContinuousInput(-Math.PI, Math.PI);
+
+        xController.reset();
+        yController.reset();
+        thetaController.reset();
+
+        xController.setTolerance(Units.inchesToMeters(0.1));
+        yController.setTolerance(Units.inchesToMeters(0.1));
+        thetaController.setTolerance(Units.degreesToRadians(1));
+
+        return Commands.run(
+                        () -> {
+                            Pose2d nearestTag = drive.getPose().nearest(reefTagPositions);
+
+                            Pose2d targetPose = alignLeft
+                                    ? nearestTag.transformBy(new Transform2d(0.66, -0.175, Rotation2d.kZero))
+                                    : nearestTag.transformBy(new Transform2d(0.66, 0.175, Rotation2d.kZero));
+
+                            xController.setSetpoint(targetPose.getX());
+                            yController.setSetpoint(targetPose.getY());
+                            thetaController.setSetpoint(targetPose
+                                    .getRotation()
+                                    .plus(Rotation2d.k180deg)
+                                    .getRadians());
+
+                            double xVal = MathUtil.clamp(
+                                    xController.calculate(drive.getPose().getX()), -1, 1);
+                            double yVal = MathUtil.clamp(
+                                    yController.calculate(drive.getPose().getY()), -1, 1);
+                            double thetaVal = MathUtil.clamp(
+                                    thetaController.calculate(
+                                            drive.getRotation().getRadians()),
+                                    -1,
+                                    1);
+
+                            Translation2d linearVelocity = getLinearVelocityFromJoysticks(
+                                    xVal * (isFlipped ? -1 : 1), yVal * (isFlipped ? -1 : 1));
+
+                            // Convert to field relative speeds & send command
+                            ChassisSpeeds speeds = new ChassisSpeeds(
+                                    linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
+                                    linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
+                                    thetaVal * drive.getMaxAngularSpeedRadPerSec());
+
+                            speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
+                                    speeds,
+                                    isFlipped
+                                            ? drive.getRotation().plus(new Rotation2d(Math.PI))
+                                            : drive.getRotation());
+                            drive.runVelocity(speeds);
+                        },
+                        drive)
+                .until(() -> xController.atSetpoint() && yController.atSetpoint() && thetaController.atSetpoint())
+                .finallyDo(() -> {
+                    xController.close();
+                    yController.close();
+                    thetaController.close();
+                });
+    }
+
     /**
      * Measures the velocity feedforward constants for the drive motors.
      *

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

@@ -0,0 +1,49 @@
+package frc.robot.commands;
+
+import static edu.wpi.first.units.Units.*;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import org.ironmaple.simulation.SimulatedArena;
+import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
+import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeCoralOnFly;
+
+public class IntakeCommands {
+
+    public static Command scoreL2Sim(SwerveDriveSimulation driveSimulation) {
+        return Commands.runOnce(() -> SimulatedArena.getInstance()
+                .addGamePieceProjectile(new ReefscapeCoralOnFly(
+                        driveSimulation.getSimulatedDriveTrainPose().getTranslation(),
+                        new Translation2d(0.55, 0),
+                        driveSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative(),
+                        driveSimulation.getSimulatedDriveTrainPose().getRotation(),
+                        Meters.of(0.9),
+                        MetersPerSecond.of(2),
+                        Degrees.of(-35))));
+    }
+
+    public static Command scoreL3Sim(SwerveDriveSimulation driveSimulation) {
+        return Commands.runOnce(() -> SimulatedArena.getInstance()
+                .addGamePieceProjectile(new ReefscapeCoralOnFly(
+                        driveSimulation.getSimulatedDriveTrainPose().getTranslation(),
+                        new Translation2d(0.55, 0),
+                        driveSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative(),
+                        driveSimulation.getSimulatedDriveTrainPose().getRotation(),
+                        Meters.of(1.33),
+                        MetersPerSecond.of(2),
+                        Degrees.of(-35))));
+    }
+
+    public static Command scoreL4Sim(SwerveDriveSimulation driveSimulation) {
+        return Commands.runOnce(() -> SimulatedArena.getInstance()
+                .addGamePieceProjectile(new ReefscapeCoralOnFly(
+                        driveSimulation.getSimulatedDriveTrainPose().getTranslation(),
+                        new Translation2d(0.66, 0),
+                        driveSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative(),
+                        driveSimulation.getSimulatedDriveTrainPose().getRotation(),
+                        Meters.of(2.1),
+                        MetersPerSecond.of(1),
+                        Degrees.of(-90))));
+    }
+}

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

@@ -0,0 +1,51 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.subsystems.drive.Drive;
+import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
+
+public class SuperstructureCommands {
+
+    public static Command scoreRightL4Sim(Drive drive, SwerveDriveSimulation driveSimulation) {
+        return Commands.sequence(
+                Commands.deadline(Commands.waitSeconds(1), DriveCommands.alignToReef(drive, false)),
+                Commands.waitSeconds(0.1),
+                IntakeCommands.scoreL4Sim(driveSimulation));
+    }
+
+    public static Command scoreLeftL4Sim(Drive drive, SwerveDriveSimulation driveSimulation) {
+        return Commands.sequence(
+                Commands.deadline(Commands.waitSeconds(1), DriveCommands.alignToReef(drive, true)),
+                Commands.waitSeconds(0.1),
+                IntakeCommands.scoreL4Sim(driveSimulation));
+    }
+
+    public static Command scoreRightL3Sim(Drive drive, SwerveDriveSimulation driveSimulation) {
+        return Commands.sequence(
+                Commands.deadline(Commands.waitSeconds(1), DriveCommands.alignToReef(drive, false)),
+                Commands.waitSeconds(0.1),
+                IntakeCommands.scoreL3Sim(driveSimulation));
+    }
+
+    public static Command scoreLeftL3Sim(Drive drive, SwerveDriveSimulation driveSimulation) {
+        return Commands.sequence(
+                Commands.deadline(Commands.waitSeconds(1), DriveCommands.alignToReef(drive, true)),
+                Commands.waitSeconds(0.1),
+                IntakeCommands.scoreL3Sim(driveSimulation));
+    }
+
+    public static Command scoreRightL2Sim(Drive drive, SwerveDriveSimulation driveSimulation) {
+        return Commands.sequence(
+                Commands.deadline(Commands.waitSeconds(1), DriveCommands.alignToReef(drive, false)),
+                Commands.waitSeconds(0.1),
+                IntakeCommands.scoreL2Sim(driveSimulation));
+    }
+
+    public static Command scoreLeftL2Sim(Drive drive, SwerveDriveSimulation driveSimulation) {
+        return Commands.sequence(
+                Commands.deadline(Commands.waitSeconds(1), DriveCommands.alignToReef(drive, true)),
+                Commands.waitSeconds(0.1),
+                IntakeCommands.scoreL2Sim(driveSimulation));
+    }
+}