Added enun to decide which DriveSubsystem to use, and fixed some errors

DriveHardware-Swerve-LeftFront-Drive-odometer.txt

@@ -0,0 +1 @@
+96.63815242633088

DriveHardware-Swerve-LeftRear-Drive-odometer.txt

@@ -0,0 +1 @@
+96.20416254778326

DriveHardware-Swerve-RightFront-Drive-odometer.txt

@@ -0,0 +1 @@
+97.89397044452848

DriveHardware-Swerve-RightRear-Drive-odometer.txt

@@ -0,0 +1 @@
+97.52366479043182

build.gradle

@@ -97,7 +97,7 @@ dependencies {
   simulationRelease wpi.sim.enableRelease()
 
   //implementation fileTree(dir: 'libs', include: '*.jar')
-  implementation 'com.github.lasarobotics:PurpleLib:2025.0.1'
+  implementation 'com.github.lasarobotics:PurpleLib:2025.0.2'
 
   implementation 'org.apache.commons:commons-math3:3.+'
   implementation 'org.tinylog:tinylog-api:2.7.0'

src/main/java/frc/robot/Constants.java

@@ -76,6 +76,11 @@ public static class SmartDashboard {
   }
 
   public static class Drive {
+    public static enum Vendor {
+      REV,
+      CTRE;
+    }
+    public static final Vendor DRIVE_VENDOR = Vendor.REV;
     public static final DriveWheel DRIVE_WHEEL = DriveWheel.create(Units.Inches.of(4.0), Units.Value.of(1.3), Units.Value.of(1.2));
     public static final PIDConstants DRIVE_PID = PIDConstants.of(0.3, 0.0, 0.001, 0.0, 0.0);
     public static final FFConstants DRIVE_FF = FFConstants.of(0.2, 0.0, 0.0, 0.0);

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

@@ -4,18 +4,22 @@
 
 package frc.robot;
 
+import org.lasarobotics.drive.swerve.SwerveDrive;
+
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 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 frc.robot.Constants.Drive.Vendor;
 import frc.robot.subsystems.WaggleSubsystem;
 import frc.robot.subsystems.drive.CTREDriveSubsystem;
 import frc.robot.subsystems.drive.REVDriveSubsystem;
 
 public class RobotContainer {
-  private static final CTREDriveSubsystem CTRE_DRIVE_SUBSYSTEM = new CTREDriveSubsystem(
+  public static final SwerveDrive DRIVE_SUBSYSTEM = (Constants.Drive.DRIVE_VENDOR.equals(Vendor.CTRE))
+  ? new CTREDriveSubsystem(
     CTREDriveSubsystem.initializeHardware(),
     Constants.Drive.DRIVE_ROTATE_PID,
     Constants.Drive.DRIVE_AUTO_AIM_PID, Constants.Drive.DRIVE_CONTROL_CENTRICITY,
@@ -24,9 +28,8 @@ public class RobotContainer {
     Constants.Drive.DRIVE_TURN_SCALAR,
     Constants.HID.CONTROLLER_DEADBAND,
     Constants.Drive.DRIVE_LOOKAHEAD
-  );
-
-  private static final REVDriveSubsystem REV_DRIVE_SUBSYSTEM = new REVDriveSubsystem(
+  ) 
+  : new REVDriveSubsystem(
     REVDriveSubsystem.initializeHardware(),
     Constants.Drive.DRIVE_ROTATE_PID,
     Constants.Drive.DRIVE_AUTO_AIM_PID, Constants.Drive.DRIVE_CONTROL_CENTRICITY,
@@ -45,16 +48,18 @@ public class RobotContainer {
 
   public RobotContainer() {
     // Set drive command
-    CTRE_DRIVE_SUBSYSTEM.setDefaultCommand(
-      CTRE_DRIVE_SUBSYSTEM.driveCommand(
+    DRIVE_SUBSYSTEM.setDefaultCommand(
+      DRIVE_SUBSYSTEM.driveCommand(
         () -> PRIMARY_CONTROLLER.getLeftY(),
         () -> PRIMARY_CONTROLLER.getLeftX(),
         () -> PRIMARY_CONTROLLER.getRightX()
       )
     );
 
     // Setup AutoBuilder
-    CTRE_DRIVE_SUBSYSTEM.configureAutoBuilder();
+    // if (Constants.Drive.DRIVE_VENDOR.equals(Vendor.CTRE))
+    //   ((CTREDriveSubsystem)DRIVE_SUBSYSTEM).configureAutoBuilder();
+    // else ((REVDriveSubsystem)DRIVE_SUBSYSTEM).configureAutoBuilder();
 
     autoModeChooser();
     SmartDashboard.putData(Constants.SmartDashboard.SMARTDASHBOARD_AUTO_MODE, m_automodeChooser);
@@ -65,9 +70,9 @@ public RobotContainer() {
 
   private void configureBindings() {
     // Start button - toggle traction control
-    PRIMARY_CONTROLLER.start().onTrue(CTRE_DRIVE_SUBSYSTEM.toggleTractionControlCommand());
+    PRIMARY_CONTROLLER.start().onTrue(DRIVE_SUBSYSTEM.toggleTractionControlCommand());
 
-    PRIMARY_CONTROLLER.povLeft().onTrue(CTRE_DRIVE_SUBSYSTEM.resetPoseCommand(() -> new Pose2d()));
+    PRIMARY_CONTROLLER.povLeft().onTrue(DRIVE_SUBSYSTEM.resetPoseCommand(() -> new Pose2d()));
 
     // Left/right bumper - wiggle stick
     PRIMARY_CONTROLLER.leftBumper().onTrue(WIGGLE_STICK.setPositionCommand(0.0));

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

@@ -7,6 +7,7 @@
 import org.apache.commons.math3.analysis.polynomials.PolynomialSplineFunction;
 import org.lasarobotics.drive.swerve.AdvancedSwerveKinematics.ControlCentricity;
 import org.lasarobotics.drive.swerve.SwerveDrive;
+import org.lasarobotics.drive.swerve.SwerveModule;
 import org.lasarobotics.drive.swerve.child.SwerveX2Module;
 import org.lasarobotics.drive.swerve.parent.CTRESwerveModule;
 import org.lasarobotics.hardware.kauailabs.NavX2;
@@ -64,8 +65,8 @@ public static Hardware initializeHardware() {
         Constants.CTREDriveHardware.LEFT_FRONT_ROTATE_MOTOR_ID,
         Constants.CTREDriveHardware.LEFT_FRONT_ROTATE_ENCODER_ID
       ),
-      org.lasarobotics.drive.swerve.SwerveModule.Location.LeftFront,
-      org.lasarobotics.drive.swerve.SwerveModule.MountOrientation.INVERTED,
+      SwerveModule.Location.LeftFront,
+      SwerveModule.MountOrientation.INVERTED,
       Constants.Drive.CTRE_GEAR_RATIO,
       Constants.Drive.DRIVE_WHEEL,
       Constants.Drive.DRIVE_PID,
@@ -86,8 +87,8 @@ public static Hardware initializeHardware() {
         Constants.CTREDriveHardware.RIGHT_FRONT_ROTATE_MOTOR_ID,
         Constants.CTREDriveHardware.RIGHT_FRONT_ROTATE_ENCODER_ID
       ),
-      org.lasarobotics.drive.swerve.SwerveModule.Location.LeftFront,
-      org.lasarobotics.drive.swerve.SwerveModule.MountOrientation.INVERTED,
+      SwerveModule.Location.RightFront,
+      SwerveModule.MountOrientation.INVERTED,
       Constants.Drive.CTRE_GEAR_RATIO,
       Constants.Drive.DRIVE_WHEEL,
       Constants.Drive.DRIVE_PID,
@@ -108,8 +109,8 @@ public static Hardware initializeHardware() {
         Constants.CTREDriveHardware.LEFT_REAR_ROTATE_MOTOR_ID,
         Constants.CTREDriveHardware.LEFT_REAR_ROTATE_ENCODER_ID
       ),
-      org.lasarobotics.drive.swerve.SwerveModule.Location.LeftFront,
-      org.lasarobotics.drive.swerve.SwerveModule.MountOrientation.INVERTED,
+      SwerveModule.Location.LeftRear,
+      SwerveModule.MountOrientation.INVERTED,
       Constants.Drive.CTRE_GEAR_RATIO,
       Constants.Drive.DRIVE_WHEEL,
       Constants.Drive.DRIVE_PID,
@@ -130,8 +131,8 @@ public static Hardware initializeHardware() {
         Constants.CTREDriveHardware.RIGHT_REAR_ROTATE_MOTOR_ID,
         Constants.CTREDriveHardware.RIGHT_REAR_ROTATE_ENCODER_ID
       ),
-      org.lasarobotics.drive.swerve.SwerveModule.Location.LeftFront,
-      org.lasarobotics.drive.swerve.SwerveModule.MountOrientation.INVERTED,
+      SwerveModule.Location.RightRear,
+      SwerveModule.MountOrientation.INVERTED,
       Constants.Drive.CTRE_GEAR_RATIO,
       Constants.Drive.DRIVE_WHEEL,
       Constants.Drive.DRIVE_PID,
@@ -167,7 +168,8 @@ public static Hardware initializeHardware() {
     return drivetrainHardware;
   }
 
-  /**Configures the AutoBuilder so that PathPlanner can use its built-in commands when running autos.
+  /**
+   * Configures the AutoBuilder so that PathPlanner can use its built-in commands when running autos.
    * Uses the PathPlanner AutoBuilder method. 
   */
 

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

@@ -15,15 +15,23 @@
 import org.lasarobotics.utils.PIDConstants;
 import org.lasarobotics.vision.AprilTagCamera;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Dimensionless;
 import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.wpilibj.DriverStation;
 import frc.robot.Constants;
 
 public class REVDriveSubsystem extends SwerveDrive {
 
+  Hardware m_drivetrainHardware;
+
   /**
    * Create an instance of DriveSubsystem
    * <p>
@@ -43,6 +51,7 @@ public REVDriveSubsystem(Hardware drivetrainHardware, PIDConstants rotatePIDF, P
                         PolynomialSplineFunction throttleInputCurve, PolynomialSplineFunction rotateInputCurve,
                         Angle rotateScalar, Dimensionless deadband, Time lookAhead) {
     super(drivetrainHardware, rotatePIDF, aimPIDF, controlCentricity, throttleInputCurve, rotateInputCurve, rotateScalar, deadband, lookAhead);
+    m_drivetrainHardware = drivetrainHardware;
   }
 
   /**
@@ -81,7 +90,7 @@ public static Hardware initializeHardware() {
         Spark.MotorKind.NEO_VORTEX,
         Spark.MotorKind.NEO_550
       ),
-      SwerveModule.Location.LeftFront,
+      SwerveModule.Location.RightFront,
       Constants.Drive.REV_GEAR_RATIO,
       Constants.Drive.DRIVE_WHEEL,
       Constants.Drive.DRIVE_PID,
@@ -103,7 +112,7 @@ public static Hardware initializeHardware() {
         Spark.MotorKind.NEO_VORTEX,
         Spark.MotorKind.NEO_550
       ),
-      org.lasarobotics.drive.swerve.SwerveModule.Location.LeftFront,
+      SwerveModule.Location.LeftRear,
       Constants.Drive.REV_GEAR_RATIO,
       Constants.Drive.DRIVE_WHEEL,
       Constants.Drive.DRIVE_PID,
@@ -125,7 +134,7 @@ public static Hardware initializeHardware() {
         Spark.MotorKind.NEO_VORTEX,
         Spark.MotorKind.NEO_550
       ),
-      org.lasarobotics.drive.swerve.SwerveModule.Location.LeftFront,
+      SwerveModule.Location.RightRear,
       Constants.Drive.REV_GEAR_RATIO,
       Constants.Drive.DRIVE_WHEEL,
       Constants.Drive.DRIVE_PID,
@@ -160,4 +169,40 @@ public static Hardware initializeHardware() {
 
     return drivetrainHardware;
   }
+
+  /**
+   * Configures the AutoBuilder so that PathPlanner can use its built-in commands when running autos.
+   * Uses the PathPlanner AutoBuilder method. 
+   */
+  public void configureAutoBuilder() {
+    AutoBuilder.configure(
+            () -> getPose(), // Robot pose supplier
+            (pose) -> resetPose(pose), // Method to reset odometry (will be called if your auto has a starting pose)
+            () -> getChassisSpeeds(), // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
+            (speeds, feedforwards) -> autoDrive(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards
+            new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains
+                    new com.pathplanner.lib.config.PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
+                    new com.pathplanner.lib.config.PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
+            ),
+            new RobotConfig(Constants.Drive.MASS, // The robot configuration
+                            Units.KilogramSquareMeters.of(1),
+                            Constants.Drive.MODULE_CONFIG,
+                            m_drivetrainHardware.lFrontModule().getModuleCoordinate(),
+                            m_drivetrainHardware.rFrontModule().getModuleCoordinate(),
+                            m_drivetrainHardware.lRearModule().getModuleCoordinate(),
+                            m_drivetrainHardware.rRearModule().getModuleCoordinate()),
+            () -> {
+              // Boolean supplier that controls when the path will be mirrored for the red alliance
+              // This will flip the path being followed to the red side of the field.
+              // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
+
+              var alliance = DriverStation.getAlliance();
+              if (alliance.isPresent()) {
+                return alliance.get() == DriverStation.Alliance.Red;
+              }
+              return false;
+            },
+            this // Reference to this subsystem to set requirements
+    );
+  }
 }

vendordeps/ThriftyLib.json

@@ -0,0 +1,20 @@
+{
+    "fileName": "ThriftyLib.json",
+    "name": "ThriftyLib",
+    "version": "2025.0.2",
+    "frcYear": "2025",
+    "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0",
+    "mavenUrls": [
+        "https://docs.home.thethriftybot.com"
+    ],
+    "jsonUrl": "https://docs.home.thethriftybot.com/ThriftyLib.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.thethriftybot.frc",
+            "artifactId": "ThriftyLib-java",
+            "version": "2025.0.2"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": []
+}