Re subsystem abstract drive

src/main/java/frc/robot/Robot.java

@@ -86,6 +86,8 @@ public void robotInit() {
 			case SIM:
 				// Running a physics simulator, log to NT
 				Logger.addDataReceiver(new NT4Publisher());
+				// Logger.addDataReceiver(new WPILOGWriter("C:\\Dev\\Robotics
+				// Related\\Controls\\2024-Rewrite\\logs", defaultPeriodSecs));
 				break;
 			case REPLAY:
 				// Replaying a log, set up replay source
@@ -103,8 +105,8 @@ public void robotInit() {
 
 		// Start AdvantageKit logger
 		Logger.start();
-
-		autoChooser = AutoBuilder.buildAutoChooser();
+		// TODO: Make default auto a "cross line" auto
+		autoChooser = AutoBuilder.buildAutoChooser("Example Auto");
 		SmartDashboard.putData("Auto Chooser", autoChooser);
 	}
 

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

@@ -13,9 +13,6 @@
 
 package frc.robot.subsystems.drive;
 
-import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
-import com.pathplanner.lib.util.ReplanningConfig;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -29,23 +26,21 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
+import frc.robot.subsystems.Subsystem;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
 import java.util.function.DoubleSupplier;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
-public class Drive extends SubsystemBase {
+public class Drive extends Subsystem<DriveStates> {
 
 	static final Lock odometryLock = new ReentrantLock();
 	private final GyroIO gyroIO;
 	private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
 	private final Module[] modules = new Module[Constants.Drive.NUM_MODULES]; // FL, FR, BL, BR
-
-	private DriveStates state;
-	private String subsystemName;
+	private PPDriveWrapper autoConfig;
 
 	private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations());
 	private Rotation2d rawGyroRotation = new Rotation2d();
@@ -70,8 +65,8 @@ public Drive(
 		ModuleIO blModuleIO,
 		ModuleIO brModuleIO
 	) {
-		subsystemName = "Drive";
-		state = DriveStates.REGULAR_DRIVE;
+		super("Drive", DriveStates.REGULAR_DRIVE);
+		autoConfig = new PPDriveWrapper(this);
 
 		this.gyroIO = gyroIO;
 		modules[0] = new Module(flModuleIO, 0);
@@ -83,26 +78,36 @@ public Drive(
 		PhoenixOdometryThread.getInstance().start();
 		SparkMaxOdometryThread.getInstance().start();
 
-		pathPlannerInit();
-	}
-
-	public void runState() {
-		drive(
-			this,
-			() -> Constants.controller.getLeftY(),
-			() -> Constants.controller.getLeftX(),
-			() -> -Constants.controller.getRightX(),
-			getState().getRotationModifier(),
-			getState().getTranslationModifier()
+		// Triggers
+		addTrigger(DriveStates.REGULAR_DRIVE, DriveStates.SLOW_MODE, () ->
+			Constants.controller.getRightBumper()
+		);
+		addTrigger(DriveStates.REGULAR_DRIVE, DriveStates.SPEED_MAXXING, () ->
+			Constants.controller.getLeftBumper()
 		);
-	}
 
-	public DriveStates getState() {
-		return state;
+		// Back to Off
+		addTrigger(DriveStates.SPEED_MAXXING, DriveStates.SLOW_MODE, () ->
+			Constants.controller.getLeftBumperReleased()
+		);
+		addTrigger(DriveStates.SLOW_MODE, DriveStates.REGULAR_DRIVE, () ->
+			Constants.controller.getRightBumperReleased()
+		);
 	}
 
-	public void setState(DriveStates state) {
-		this.state = state;
+	@Override
+	public void runState() {
+		// Can't run in auto otherwise it will constantly tell drive not to drive in auto (and thats not good)
+		if (DriverStation.isTeleop()) {
+			drive(
+				this,
+				() -> Constants.controller.getLeftY(),
+				() -> Constants.controller.getLeftX(),
+				() -> -Constants.controller.getRightX(),
+				getState().getRotationModifier(),
+				getState().getTranslationModifier()
+			);
+		}
 	}
 
 	public void drive(
@@ -212,18 +217,6 @@ public void periodic() {
 			// Apply update
 			poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions);
 		}
-
-		if (Constants.controller.getRightBumper() && getState() == DriveStates.REGULAR_DRIVE) {
-			setState(DriveStates.SLOW_MODE);
-		} else if (
-			Constants.controller.getLeftBumper() && getState() == DriveStates.REGULAR_DRIVE
-		) {
-			setState(DriveStates.SPEED_MAXXING);
-		} else if (
-			!Constants.controller.getRightBumper() && !Constants.controller.getLeftBumper()
-		) {
-			setState(DriveStates.REGULAR_DRIVE);
-		}
 	}
 
 	/**
@@ -363,36 +356,4 @@ public static Translation2d[] getModuleTranslations() {
 			),
 		};
 	}
-
-	public void pathPlannerInit() {
-		AutoBuilder.configureHolonomic(
-			this::getPose, // Robot pose supplier
-			this::setPose, // Method to reset odometry (will be called if your auto has a starting pose)
-			this::getChassisSpeed, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
-			this::runVelocity, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds
-			new HolonomicPathFollowerConfig(
-				// HolonomicPathFollowerConfig, this should likely live in
-				// your Constants class
-				Constants.Drive.TRANSLATION_PID, // Translation PID constants
-				Constants.Drive.ROTATION_PID, // Rotation PID constants
-				Constants.Drive.MAX_MODULE_SPEED, // Max module speed, in m/s
-				Constants.Drive.DRIVE_BASE_RADIUS, // Drive base radius in meters. Distance from robot center to
-				// furthest module.
-				new ReplanningConfig() // Default path replanning config. See the API for the options
-				// here
-			),
-			() -> {
-				// 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
-		);
-	}
 }

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

@@ -0,0 +1,71 @@
+package frc.robot.subsystems.drive;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
+import com.pathplanner.lib.util.ReplanningConfig;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class PPDriveWrapper extends SubsystemBase {
+
+	Drive drive;
+
+	PPDriveWrapper(Drive drive) {
+		this.drive = drive;
+		pathPlannerInit();
+	}
+
+	private Pose2d getPose() {
+		return drive.getPose();
+	}
+
+	private void setPose(Pose2d pose) {
+		drive.setPose(pose);
+	}
+
+	private ChassisSpeeds getChassisSpeed() {
+		return drive.getChassisSpeed();
+	}
+
+	private void runVelocity(ChassisSpeeds chassisSpeeds) {
+		drive.runVelocity(chassisSpeeds);
+	}
+
+	public void periodic() {}
+
+	public void pathPlannerInit() {
+		AutoBuilder.configureHolonomic(
+			this::getPose, // Robot pose supplier
+			this::setPose, // Method to reset odometry (will be called if your auto has a starting pose)
+			this::getChassisSpeed, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
+			this::runVelocity, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds
+			new HolonomicPathFollowerConfig(
+				// HolonomicPathFollowerConfig, this should likely live in
+				// your Constants class
+				Constants.Drive.TRANSLATION_PID, // Translation PID constants
+				Constants.Drive.ROTATION_PID, // Rotation PID constants
+				Constants.Drive.MAX_MODULE_SPEED, // Max module speed, in m/s
+				Constants.Drive.DRIVE_BASE_RADIUS, // Drive base radius in meters. Distance from robot center to
+				// furthest module.
+				// Replans path if vision or odo detects errors (S tier)
+				new ReplanningConfig(true, true) // Default path replanning config. See the API for the options
+				// here
+			),
+			() -> {
+				// 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
+		);
+	}
+}