Fixed bugs

Author: SalmonDebug

src/main/java/frc/robot/Autos.java

@@ -4,18 +4,19 @@
 import frc.robot.subsystems.drive.Drive;
 
 public class Autos {
-    AutoFactory autoFactory;
-    Drive drivesubsystem;
+  AutoFactory autoFactory;
+  Drive drivesubsystem;
 
-  public Autos(
-    Drive drivesubsystem) {
-        autoFactory = new AutoFactory(
-        drivesubsystem::getPose, // A function that returns the current robot pose
-        drivesubsystem::resetOdometry, // A function that resets the current robot pose to the provided Pose2d
-        drivesubsystem::followTrajectory, // The drive subsystem trajectory follower 
-true, // If alliance flipping should be enabled 
-        drivesubsystem, // The drive subsystem
-    );
+  public Autos(Drive drivesubsystem) {
+    autoFactory =
+        new AutoFactory(
+            drivesubsystem::getPose, // A function that returns the current robot pose
+            drivesubsystem
+                ::setPose, // A function that resets the current robot pose to the provided Pose2d
+            drivesubsystem::followTrajectory, // The drive subsystem trajectory follower
+            true, // If alliance flipping should be enabled
+            drivesubsystem // The drive subsystem
+            );
     this.drivesubsystem = drivesubsystem;
   }
 }

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

@@ -7,15 +7,16 @@
 
 package frc.robot;
 
-
+import choreo.auto.AutoChooser;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 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 edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import frc.robot.commands.DriveCommands;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.drive.Drive;
@@ -24,10 +25,6 @@
 import frc.robot.subsystems.drive.ModuleIO;
 import frc.robot.subsystems.drive.ModuleIOSim;
 import frc.robot.subsystems.drive.ModuleIOTalonFX;
-import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
-
-import choreo.auto.AutoChooser;
-import choreo.auto.AutoFactory;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -43,8 +40,8 @@ public class RobotContainer {
   private final CommandXboxController controller = new CommandXboxController(0);
 
   // Dashboard inputs
-//  private final LoggedDashboardChooser<Command> autoChooser;
-private final AutoChooser autoChooser;
+  //  private final LoggedDashboardChooser<Command> autoChooser;
+  private final AutoChooser autoChooser;
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     switch (Constants.currentMode) {
@@ -103,23 +100,11 @@ public RobotContainer() {
     }
 
     // Set up auto routines
-  //  autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoFactory.buildAutoChooser());
-autoChooser = new AutoChooser();
+    //  autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoFactory.buildAutoChooser());
+    autoChooser = new AutoChooser();
     // Set up SysId routines
-    autoChooser.addCmd(
-        "Drive Wheel Radius Characterization", DriveCommands.wheelRadiusCharacterization(drive));
-    autoChooser.addCmd(
-        "Drive Simple FF Characterization", DriveCommands.feedforwardCharacterization(drive));
-    autoChooser.addOption(
-        "Drive SysId (Quasistatic Forward)",
-        drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
-    autoChooser.addOption(
-        "Drive SysId (Quasistatic Reverse)",
-        drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
-    autoChooser.addOption(
-        "Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
-    autoChooser.addOption(
-        "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+    SmartDashboard.putData(autoChooser);
+    RobotModeTriggers.autonomous().whileTrue(autoChooser.selectedCommandScheduler());
 
     // Configure the button bindings
     configureButtonBindings();
@@ -171,6 +156,6 @@ private void configureButtonBindings() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    return autoChooser.get();
+    return autoChooser.selectedCommand();
   }
 }

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -9,13 +9,7 @@
 
 import static edu.wpi.first.units.Units.*;
 
-// import com.pathplanner.lib.auto.AutoBuilder;
-// import com.pathplanner.lib.config.ModuleConfig;
-// import com.pathplanner.lib.config.PIDConstants;
-// import com.pathplanner.lib.config.RobotConfig;
-// import com.pathplanner.lib.controllers.PPHolonomicDriveController;
-// import com.pathplanner.lib.pathfinding.Pathfinding;
-// import com.pathplanner.lib.util.PathPlannerLogging;
+import choreo.trajectory.SwerveSample;
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
@@ -32,31 +26,26 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
 import frc.robot.Constants.Mode;
 import frc.robot.generated.TunerConstants;
-//import frc.robot.util.LocalADStarAK;
+// import frc.robot.util.LocalADStarAK;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
-import choreo.auto.AutoFactory;
-import choreo.trajectory.SwerveSample;
-
 public class Drive extends SubsystemBase {
   private final PIDController xController = new PIDController(10.0, 0.0, 0.0);
   private final PIDController yController = new PIDController(10.0, 0.0, 0.0);
   private final PIDController headingController = new PIDController(7.5, 0.0, 0.0);
-  
+
   // TunerConstants doesn't include these constants, so they are declared locally
   static final double ODOMETRY_FREQUENCY = TunerConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0;
   public static final double DRIVE_BASE_RADIUS =
@@ -117,7 +106,7 @@ public Drive(
     modules[1] = new Module(frModuleIO, 1, TunerConstants.FrontRight);
     modules[2] = new Module(blModuleIO, 2, TunerConstants.BackLeft);
     modules[3] = new Module(brModuleIO, 3, TunerConstants.BackRight);
-    
+
     headingController.enableContinuousInput(-Math.PI, Math.PI);
 
     // Usage reporting for swerve template
@@ -147,7 +136,6 @@ public Drive(
     //       Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
     //     });
 
-
     // Configure SysId
     sysId =
         new SysIdRoutine(
@@ -160,19 +148,18 @@ public Drive(
                 (voltage) -> runCharacterization(voltage.in(Volts)), null, this));
   }
 
-      public void followTrajectory(SwerveSample sample) {
-        // Get the current pose of the robot
-        Pose2d pose = getPose();
+  public void followTrajectory(SwerveSample sample) {
+    // Get the current pose of the robot
+    Pose2d pose = getPose();
 
-        // Generate the next speeds for the robot
-        ChassisSpeeds speeds = new ChassisSpeeds(
+    // Generate the next speeds for the robot
+    ChassisSpeeds speeds =
+        new ChassisSpeeds(
             sample.vx + xController.calculate(pose.getX(), sample.x),
             sample.vy + yController.calculate(pose.getY(), sample.y),
-            sample.omega + headingController.calculate(pose.getRotation().getRadians(), sample.heading)
-        );
-
-
-    }  
+            sample.omega
+                + headingController.calculate(pose.getRotation().getRadians(), sample.heading));
+  }
 
   @Override
   public void periodic() {

src/main/java/frc/robot/util/LocalADStarAK.java

@@ -82,7 +82,8 @@
 //   /**
 //    * Set the goal position to pathfind to
 //    *
-//    * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved
+//    * @param goalPosition Goal position on the field. f this is within an obstacle it will be
+// moved
 //    *     to the nearest non-obstacle node.
 //    */
 //   @Override
@@ -95,7 +96,8 @@
 //   /**
 //    * Set the dynamic obstacles that should be avoided while pathfinding.
 //    *
-//    * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents
+//    * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d
+// represents
 //    *     opposite corners of a bounding box.
 //    * @param currentRobotPos The current position of the robot. This is needed to change the start
 //    *     position of the path to properly avoid obstacles

vendordeps/photonlib.json

@@ -1,7 +1,7 @@
 {
   "fileName": "photonlib.json",
   "name": "photonlib",
-  "version": "v2026.1.1-rc-3",
+  "version": "v2026.1.1",
   "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
   "frcYear": "2026",
   "mavenUrls": [
@@ -13,7 +13,7 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photontargeting-cpp",
-      "version": "v2026.1.1-rc-3",
+      "version": "v2026.1.1",
       "skipInvalidPlatforms": true,
       "isJar": false,
       "validPlatforms": [
@@ -28,7 +28,7 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photonlib-cpp",
-      "version": "v2026.1.1-rc-3",
+      "version": "v2026.1.1",
       "libName": "photonlib",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -43,7 +43,7 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photontargeting-cpp",
-      "version": "v2026.1.1-rc-3",
+      "version": "v2026.1.1",
       "libName": "photontargeting",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -60,12 +60,12 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photonlib-java",
-      "version": "v2026.1.1-rc-3"
+      "version": "v2026.1.1"
     },
     {
       "groupId": "org.photonvision",
       "artifactId": "photontargeting-java",
-      "version": "v2026.1.1-rc-3"
+      "version": "v2026.1.1"
     }
   ]
 }