Maple Subsystem dev

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

@@ -30,6 +30,14 @@ public enum RobotMode {
         REPLAY
     }
 
+    public static final class LogConfigs {
+        // avoid typos
+        public static final String
+                SENSORS_INPUTS_PATH = "RawInputs/",
+                SENSORS_PROCESSED_INPUTS_PATH = "ProcessedInputs/",
+                SYSTEM_PERFORMANCE_PATH = "SystemPerformance/";
+    }
+
     public static final class ChassisConfigs {
         public enum ChassisType {
             REV,

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

@@ -6,6 +6,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.subsystems.MapleSubsystem;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
@@ -75,6 +76,7 @@ public void robotInit() {
     public void robotPeriodic() {
         if (this.isTest())
             return;
+        MapleSubsystem.checkForOnDisableAndEnable();
         CommandScheduler.getInstance().run();
     }
 

src/main/java/frc/robot/subsystems/MapleSubsystem.java

@@ -0,0 +1,81 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+import frc.robot.Robot;
+import frc.robot.utils.MapleTimeUtils;
+import org.littletonrobotics.junction.Logger;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public abstract class MapleSubsystem extends SubsystemBase {
+    public static final List<MapleSubsystem> instances = new ArrayList<>();
+    private double previousUpdateTimeStamp = 0;
+    public static void register(MapleSubsystem instance) {
+        instances.add(instance);
+    }
+    public static void cancelRegister(MapleSubsystem instance) {
+        instances.remove(instance);
+    }
+
+    public static void subsystemsInit() {
+        for (MapleSubsystem instance:instances)
+            instance.onReset();
+    }
+
+    private static boolean wasEnabled = false;
+    public static void checkForOnDisableAndEnable() {
+        // periodic() is called from CommandScheduler, we only need to check for enable/disable
+        if (DriverStation.isEnabled() && (!wasEnabled))
+            enableSubsystems();
+        else if (DriverStation.isDisabled() && (wasEnabled))
+            disableSubsystems();
+        wasEnabled = DriverStation.isEnabled();
+    }
+    private static void enableSubsystems() {
+        for (MapleSubsystem instance:instances)
+            if (!instance.updateDuringDisabled)
+                instance.onEnable();
+    }
+
+    private static void disableSubsystems() {
+        for (MapleSubsystem instance:instances)
+            if (!instance.updateDuringDisabled)
+                instance.onDisable();
+    }
+
+    private final boolean updateDuringDisabled;
+    public MapleSubsystem(String name) {
+        this(name, false);
+    }
+    public MapleSubsystem(String name, boolean updateDuringDisabled) {
+        super(name);
+        this.updateDuringDisabled = updateDuringDisabled;
+        register(this);
+    }
+
+    public void onEnable() {}
+    public void onDisable() {}
+    public abstract void onReset();
+    public abstract void periodic(double dt, boolean enabled);
+
+    @Override
+    public void periodic() {
+        final long t0 = System.nanoTime();
+        periodic(getDt(), DriverStation.isEnabled());
+        final double cpuTimeMS = (System.nanoTime() - t0) / 1_000_000.0;
+        Logger.recordOutput(Constants.LogConfigs.SYSTEM_PERFORMANCE_PATH + getName() + "-CPUTimeMS", cpuTimeMS);
+    }
+
+    private double getDt() {
+        if (previousUpdateTimeStamp == 0) {
+            previousUpdateTimeStamp = MapleTimeUtils.getLogTimeSeconds();
+            return Robot.defaultPeriodSecs;
+        }
+        final double dt = MapleTimeUtils.getLogTimeSeconds() - previousUpdateTimeStamp;
+        previousUpdateTimeStamp = Logger.getTimestamp();
+        return dt;
+    }
+}

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

@@ -24,14 +24,15 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.Robot;
+import frc.robot.subsystems.MapleSubsystem;
 import frc.robot.utils.LocalADStarAK;
 
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
 import java.util.Arrays;
 
-public class Drive extends SubsystemBase {
+public class Drive extends MapleSubsystem {
     private static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5);
     private static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0);
     private static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0);
@@ -63,6 +64,7 @@ public Drive(
             ModuleIO frModuleIO,
             ModuleIO blModuleIO,
             ModuleIO brModuleIO) {
+        super("Drive");
         this.gyroIO = gyroIO;
         modules[0] = new Module(flModuleIO, 0);
         modules[1] = new Module(frModuleIO, 1);
@@ -93,7 +95,13 @@ MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()),
                 (targetPose) -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose));
     }
 
-    public void periodic() {
+    @Override
+    public void onReset() {
+
+    }
+
+    @Override
+    public void periodic(double dt, boolean enabled) {
         odometryThread.updateInputs(odometryThreadInputs);
         Logger.processInputs("Drive/OdometryThread", odometryThreadInputs);
         gyroIO.updateInputs(gyroInputs);

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

@@ -10,10 +10,12 @@
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.Robot;
+import frc.robot.subsystems.MapleSubsystem;
 import org.littletonrobotics.junction.Logger;
 
-public class Module {
+public class Module extends MapleSubsystem {
     private static final double WHEEL_RADIUS = Units.inchesToMeters(2.0);
     static final double ODOMETRY_FREQUENCY = 250.0;
 
@@ -30,6 +32,7 @@ public class Module {
     private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[]{};
 
     public Module(ModuleIO io, int index) {
+        super("Module" + index);
         this.io = io;
         this.index = index;
 
@@ -56,9 +59,17 @@ public Module(ModuleIO io, int index) {
 
         turnFeedback.enableContinuousInput(-Math.PI, Math.PI);
         setBrakeMode(true);
+
+        CommandScheduler.getInstance().unregisterSubsystem(this);
+    }
+
+    @Override
+    public void onReset() {
+
     }
 
-    public void periodic() {
+    @Override
+    public void periodic(double dt, boolean enabled) {
         io.updateInputs(inputs);
         Logger.processInputs("Drive/Module" + index, inputs);