From 618c23eaaf3bb5c120fc9c2a70d33db851692523 Mon Sep 17 00:00:00 2001 From: Ian Galaviz Date: Fri, 2 Jan 2026 13:14:06 -0700 Subject: [PATCH 1/2] robot can now automatically score on all L3 and L4 --- build.gradle | 4 +- src/main/java/frc/robot/FieldConstants.java | 33 +++++++ src/main/java/frc/robot/RobotContainer.java | 50 +++++------ .../frc/robot/commands/DriveCommands.java | 90 ++++++++++++++++++- .../frc/robot/commands/IntakeCommands.java | 37 ++++++++ .../commands/SuperstructureCommands.java | 37 ++++++++ 6 files changed, 218 insertions(+), 33 deletions(-) create mode 100644 src/main/java/frc/robot/FieldConstants.java create mode 100644 src/main/java/frc/robot/commands/IntakeCommands.java create mode 100644 src/main/java/frc/robot/commands/SuperstructureCommands.java diff --git a/build.gradle b/build.gradle index 0a5591a..ab882c2 100644 --- a/build.gradle +++ b/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 diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java new file mode 100644 index 0000000..5a4fd69 --- /dev/null +++ b/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 tagPositions = new ArrayList(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()); + } + } +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5cd96c9..c517afb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,14 +13,12 @@ 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; @@ -28,11 +26,11 @@ 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 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,21 @@ 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()); } /** diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 1d7ea98..9703625 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/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 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. * diff --git a/src/main/java/frc/robot/commands/IntakeCommands.java b/src/main/java/frc/robot/commands/IntakeCommands.java new file mode 100644 index 0000000..597a7c0 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeCommands.java @@ -0,0 +1,37 @@ +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 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)))); + } +} diff --git a/src/main/java/frc/robot/commands/SuperstructureCommands.java b/src/main/java/frc/robot/commands/SuperstructureCommands.java new file mode 100644 index 0000000..043dbf1 --- /dev/null +++ b/src/main/java/frc/robot/commands/SuperstructureCommands.java @@ -0,0 +1,37 @@ +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)); + } +} From 2596a01cf36c88a50af0600296b652f569806639 Mon Sep 17 00:00:00 2001 From: Ian Galaviz Date: Fri, 2 Jan 2026 13:26:50 -0700 Subject: [PATCH 2/2] robot can now automatically score on all L2 --- src/main/java/frc/robot/RobotContainer.java | 10 ++++++++++ .../java/frc/robot/commands/IntakeCommands.java | 12 ++++++++++++ .../frc/robot/commands/SuperstructureCommands.java | 14 ++++++++++++++ 3 files changed, 36 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c517afb..466eaa3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -174,6 +174,16 @@ private void configureButtonBindings() { 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()); } /** diff --git a/src/main/java/frc/robot/commands/IntakeCommands.java b/src/main/java/frc/robot/commands/IntakeCommands.java index 597a7c0..4301668 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands.java +++ b/src/main/java/frc/robot/commands/IntakeCommands.java @@ -11,6 +11,18 @@ 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( diff --git a/src/main/java/frc/robot/commands/SuperstructureCommands.java b/src/main/java/frc/robot/commands/SuperstructureCommands.java index 043dbf1..c098da6 100644 --- a/src/main/java/frc/robot/commands/SuperstructureCommands.java +++ b/src/main/java/frc/robot/commands/SuperstructureCommands.java @@ -34,4 +34,18 @@ public static Command scoreLeftL3Sim(Drive drive, SwerveDriveSimulation driveSim 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)); + } }