Update to 2026.2.1, more auto code, more tag tracking code, pondering on the fly path generation

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<Command> 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");
+    }
   }
 }

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;
+        }
+    }
+}

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;
+}

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<DriverStation.Alliance> 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;