From 929dd8132939bcfc26cd7fd009468a1f4278c567 Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Sun, 18 Jan 2026 23:01:17 -0500 Subject: [PATCH] Update to 2026.2.1, more auto code, more tag tracking code, pondering on the fly path generation --- build.gradle | 2 +- src/main/java/frc/robot/RobotContainer.java | 16 +++- .../frc/robot/constants/AutoConstants.java | 62 ++++++++++++++ .../frc/robot/constants/PhotonConstants.java | 17 ++++ .../java/frc/robot/subsystems/Drivetrain.java | 81 +++++++++++++++++++ 5 files changed, 175 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/constants/AutoConstants.java diff --git a/build.gradle b/build.gradle index 47f2286..bee7304 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2026.1.1" + id "edu.wpi.first.GradleRIO" version "2026.2.1" } java { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a6a2556..30b12ab 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,9 +4,13 @@ package frc.robot; +import com.pathplanner.lib.auto.AutoBuilder; + +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.constants.AutoConstants; import frc.robot.constants.OIConstants; import frc.robot.subsystems.Drivetrain; @@ -15,11 +19,15 @@ public class RobotContainer { private CommandXboxController driver; + private SendableChooser autoChooser; + public RobotContainer() { drivetrain = new Drivetrain(); driver = new CommandXboxController(OIConstants.kDriverControllerPort); + autoChooser = AutoBuilder.buildAutoChooser(); + configureBindings(); } @@ -35,6 +43,10 @@ public class RobotContainer { } public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + if(AutoConstants.kAutoConfigOk) { + return autoChooser.getSelected(); + } else { + return new PrintCommand("Robot Config loading failed, autonomous disabled"); + } } } diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java new file mode 100644 index 0000000..e2e8139 --- /dev/null +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -0,0 +1,62 @@ +package frc.robot.constants; + +import java.io.IOException; + +import org.json.simple.parser.ParseException; + +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.path.PathConstraints; + +import edu.wpi.first.math.trajectory.TrapezoidProfile; + +// TODO This is all hold over from 2025, does any of it need to change? +public class AutoConstants { + public static final double kMaxSpeedMetersPerSecond = 5; + public static final double kMaxAccelerationMetersPerSecondSquared = 4; + public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; + public static final double kMaxAngularAccelerationRadiansPerSecondSquared = Math.PI; + + public static final double kMaxSpeedMetersPerSecondAutoAlign = 2.5; + + public static final double kPXYController = 3.5; + public static final double kPThetaController = 5; + + public static final double kAlignPXYController = 2; + public static final double kAlignPThetaController = 5; + + // Constraint for the motion profiled robot angle controller + public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( + kMaxAngularSpeedRadiansPerSecond, kMaxAngularAccelerationRadiansPerSecondSquared); + + public static final TrapezoidProfile.Constraints kAlignThetaControllerConstraints = new TrapezoidProfile.Constraints( + kMaxAngularSpeedRadiansPerSecond, kMaxAngularAccelerationRadiansPerSecondSquared); + + // TODO This is a constant being managed like a static rewriteable variable + public static RobotConfig kRobotConfig; + public static boolean kAutoConfigOk; + + public static final PPHolonomicDriveController kPPDriveController = new PPHolonomicDriveController( + new PIDConstants(kPXYController, 0, 0), + new PIDConstants(kPThetaController, 0, 0) + ); + + public static final PathConstraints kOnTheFlyConstraints = new PathConstraints( + kMaxSpeedMetersPerSecond, + kMaxAccelerationMetersPerSecondSquared, + kMaxAngularSpeedRadiansPerSecond, + kMaxAngularAccelerationRadiansPerSecondSquared + ); + + static { + try { + kRobotConfig = RobotConfig.fromGUISettings(); + kAutoConfigOk = true; + } catch (IOException | ParseException e) { + System.err.println("FAILED TO READ ROBOTCONFIG, WAS THE CONFIG SET UP IN PATHPLANNER?"); + e.printStackTrace(); + kAutoConfigOk = false; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/PhotonConstants.java b/src/main/java/frc/robot/constants/PhotonConstants.java index e69de29..a90ec54 100644 --- a/src/main/java/frc/robot/constants/PhotonConstants.java +++ b/src/main/java/frc/robot/constants/PhotonConstants.java @@ -0,0 +1,17 @@ +package frc.robot.constants; + +import edu.wpi.first.math.geometry.Transform3d; + +public class PhotonConstants { + public static final String kCamera1Name = "pv1"; + public static final String kCamera2Name = "pv2"; + + // TODO Need actual values for all of this + public static final Transform3d kCamera1RobotToCam = new Transform3d(); + public static final Transform3d kCamera2RobotToCam = new Transform3d(); + + public static final double kCamera1HeightMeters = 0; + public static final double kCamera1PitchRadians = 0; + public static final double kCamera2HeightMeters = 0; + public static final double kCamera2PitchRadians = 0; +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 9df0d78..4077106 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -1,14 +1,19 @@ package frc.robot.subsystems; +import java.util.Optional; +import java.util.OptionalDouble; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; import com.studica.frc.AHRS; import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -16,10 +21,15 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.PIDCommand; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.AutoConstants; import frc.robot.constants.DrivetrainConstants; import frc.robot.constants.OIConstants; +import frc.robot.utilities.PhotonVision; import frc.robot.utilities.SwerveModule; public class Drivetrain extends SubsystemBase { @@ -32,6 +42,9 @@ public class Drivetrain extends SubsystemBase { private SwerveDrivePoseEstimator estimator; + private PhotonVision camera1; + private PhotonVision camera2; + public Drivetrain() { frontLeft = new SwerveModule( "FrontLeft", @@ -79,6 +92,25 @@ public class Drivetrain extends SubsystemBase { }, new Pose2d() ); + + if(AutoConstants.kAutoConfigOk) { + AutoBuilder.configure( + this::getPose, + this::resetOdometry, + this::getCurrentChassisSpeeds, + (speeds, feedforwards) -> driveWithChassisSpeeds(speeds), + AutoConstants.kPPDriveController, + AutoConstants.kRobotConfig, + () -> { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this + ); + } } @Override @@ -102,6 +134,42 @@ public class Drivetrain extends SubsystemBase { Logger.recordOutput("Drivetrain/Heading", getHeading()); } + // TODO check both cameras + public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) { + if (camera1 == null) { + return new PrintCommand("Camera 1 not available"); + } + + // TODO The process variable is different here than what these constants are used for, may need to use something different + PIDController controller = new PIDController( + AutoConstants.kPThetaController, + 0, + 0 + ); + + return runOnce(controller::reset).andThen( + drive( + xSpeed, + ySpeed, + () -> { + OptionalDouble tagYaw = camera1.getTagYawByID(tagID); + + return (tagYaw.isEmpty() ? 0 : controller.calculate(tagYaw.getAsDouble(), 0)); + }, + () -> false + ) + ); + } + + public Command drivePathPlannerPath(PathPlannerPath path) { + if(AutoConstants.kAutoConfigOk) { + return AutoBuilder.followPath(path); + } else { + return new PrintCommand("Robot Config loading failed, on the fly PathPlanner disabled"); + } + + } + public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, BooleanSupplier fieldRelative) { // TODO Inversions? Specific Alliance code? return run(() -> { @@ -130,6 +198,19 @@ public class Drivetrain extends SubsystemBase { rearRight.resetEncoders(); } + public void resetOdometry(Pose2d pose) { + estimator.resetPosition( + Rotation2d.fromDegrees(getGyroValue()), + new SwerveModulePosition[] { + frontLeft.getPosition(), + frontRight.getPosition(), + rearLeft.getPosition(), + rearRight.getPosition() + }, + pose + ); + } + public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRelative) { double p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2)); double xSpeedDelivered = 0;