diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ce9e4839..aa01bdff 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -301,19 +301,9 @@ public void robotPeriodic() { // Profiling output if (Constants.PROFILING_ENABLED) { - long schedulerMs = (t1 - loopStart) / 1_000_000; - long gameStateMs = (t2 - t1) / 1_000_000; - long totalMs = (t2 - loopStart) / 1_000_000; - if (totalMs > 20) { - System.out.println( - "[Robot] scheduler=" - + schedulerMs - + "ms gameState=" - + gameStateMs - + "ms total=" - + totalMs - + "ms"); - } + Logger.recordOutput("Profiling/Robot/SchedulerMs", (t1 - loopStart) / 1_000_000); + Logger.recordOutput("Profiling/Robot/GameStateMs", (t2 - t1) / 1_000_000); + Logger.recordOutput("Profiling/Robot/TotalMs", (t2 - loopStart) / 1_000_000); } // Return to non-RT thread priority (do not modify the first argument) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 1e0ca4a9..1320441d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -222,25 +222,13 @@ public void periodic() { // Profiling output if (Constants.PROFILING_ENABLED) { - long totalMs = (t6 - startNanos) / 1_000_000; - if (totalMs > 5) { - System.out.println( - "[Drive] lock=" - + (t1 - startNanos) / 1_000_000 - + "ms gyroUpdate=" - + (t2 - t1) / 1_000_000 - + "ms gyroLog=" - + (t3 - t2) / 1_000_000 - + "ms modules=" - + (t4 - t3) / 1_000_000 - + "ms disabled=" - + (t5 - t4) / 1_000_000 - + "ms odometry=" - + (t6 - t5) / 1_000_000 - + "ms total=" - + totalMs - + "ms"); - } + Logger.recordOutput("Profiling/Drive/LockMs", (t1 - startNanos) / 1_000_000); + Logger.recordOutput("Profiling/Drive/GyroUpdateMs", (t2 - t1) / 1_000_000); + Logger.recordOutput("Profiling/Drive/GyroLogMs", (t3 - t2) / 1_000_000); + Logger.recordOutput("Profiling/Drive/ModulesMs", (t4 - t3) / 1_000_000); + Logger.recordOutput("Profiling/Drive/DisabledMs", (t5 - t4) / 1_000_000); + Logger.recordOutput("Profiling/Drive/OdometryMs", (t6 - t5) / 1_000_000); + Logger.recordOutput("Profiling/Drive/TotalMs", (t6 - startNanos) / 1_000_000); } } diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index a4c581af..1b1e70f5 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -76,19 +76,10 @@ public void periodic() { // Profiling output if (Constants.PROFILING_ENABLED) { - long totalMs = (t3 - t0) / 1_000_000; - if (totalMs > 2) { - System.out.println( - "[Module" - + index - + "] updateInputs=" - + (t1 - t0) / 1_000_000 - + "ms log=" - + (t2 - t1) / 1_000_000 - + "ms rest=" - + (t3 - t2) / 1_000_000 - + "ms"); - } + Logger.recordOutput("Profiling/Module" + index + "/UpdateInputsMs", (t1 - t0) / 1_000_000); + Logger.recordOutput("Profiling/Module" + index + "/LogMs", (t2 - t1) / 1_000_000); + Logger.recordOutput("Profiling/Module" + index + "/RestMs", (t3 - t2) / 1_000_000); + Logger.recordOutput("Profiling/Module" + index + "/TotalMs", (t3 - t0) / 1_000_000); } } diff --git a/src/main/java/frc/robot/subsystems/feeder/Feeder.java b/src/main/java/frc/robot/subsystems/feeder/Feeder.java index 1c94bc9d..808a63a4 100644 --- a/src/main/java/frc/robot/subsystems/feeder/Feeder.java +++ b/src/main/java/frc/robot/subsystems/feeder/Feeder.java @@ -45,21 +45,11 @@ public void periodic() { // Profiling output if (Constants.PROFILING_ENABLED) { - long totalMs = (t4 - t0) / 1_000_000; - if (totalMs > 2) { - System.out.println( - "[Feeder] spindexer=" - + (t1 - t0) / 1_000_000 - + "ms kicker=" - + (t2 - t1) / 1_000_000 - + "ms spindexerLog=" - + (t3 - t2) / 1_000_000 - + "ms kickerLog=" - + (t4 - t3) / 1_000_000 - + "ms total=" - + totalMs - + "ms"); - } + Logger.recordOutput("Profiling/Feeder/SpindexerMs", (t1 - t0) / 1_000_000); + Logger.recordOutput("Profiling/Feeder/KickerMs", (t2 - t1) / 1_000_000); + Logger.recordOutput("Profiling/Feeder/SpindexerLogMs", (t3 - t2) / 1_000_000); + Logger.recordOutput("Profiling/Feeder/KickerLogMs", (t4 - t3) / 1_000_000); + Logger.recordOutput("Profiling/Feeder/TotalMs", (t4 - t0) / 1_000_000); } } diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java index f62d15b2..3291d406 100644 --- a/src/main/java/frc/robot/subsystems/hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/hopper/Hopper.java @@ -25,17 +25,9 @@ public void periodic() { // Profiling output if (Constants.PROFILING_ENABLED) { - long totalMs = (t2 - t0) / 1_000_000; - if (totalMs > 2) { - System.out.println( - "[Hopper] update=" - + (t1 - t0) / 1_000_000 - + "ms log=" - + (t2 - t1) / 1_000_000 - + "ms total=" - + totalMs - + "ms"); - } + Logger.recordOutput("Profiling/Hopper/UpdateMs", (t1 - t0) / 1_000_000); + Logger.recordOutput("Profiling/Hopper/LogMs", (t2 - t1) / 1_000_000); + Logger.recordOutput("Profiling/Hopper/TotalMs", (t2 - t0) / 1_000_000); } } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 90090a22..6f936d79 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -51,17 +51,9 @@ public void periodic() { // Profiling output if (Constants.PROFILING_ENABLED) { - long totalMs = (t2 - t0) / 1_000_000; - if (totalMs > 2) { - System.out.println( - "[Intake] update=" - + (t1 - t0) / 1_000_000 - + "ms log=" - + (t2 - t1) / 1_000_000 - + "ms total=" - + totalMs - + "ms"); - } + Logger.recordOutput("Profiling/Intake/UpdateMs", (t1 - t0) / 1_000_000); + Logger.recordOutput("Profiling/Intake/LogMs", (t2 - t1) / 1_000_000); + Logger.recordOutput("Profiling/Intake/TotalMs", (t2 - t0) / 1_000_000); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index 5f46e7b9..ffa1a74b 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -142,21 +142,11 @@ public void periodic() { // Profiling output if (Constants.PROFILING_ENABLED) { - long totalMs = (t4 - t0) / 1_000_000; - if (totalMs > 3) { - System.out.println( - "[Launcher] update=" - + (t1 - t0) / 1_000_000 - + "ms log=" - + (t2 - t1) / 1_000_000 - + "ms aimLog=" - + (t3 - t2) / 1_000_000 - + "ms ballistics=" - + (t4 - t3) / 1_000_000 - + "ms total=" - + totalMs - + "ms"); - } + Logger.recordOutput("Profiling/Launcher/UpdateMs", (t1 - t0) / 1_000_000); + Logger.recordOutput("Profiling/Launcher/LogMs", (t2 - t1) / 1_000_000); + Logger.recordOutput("Profiling/Launcher/AimLogMs", (t3 - t2) / 1_000_000); + Logger.recordOutput("Profiling/Launcher/BallisticsMs", (t4 - t3) / 1_000_000); + Logger.recordOutput("Profiling/Launcher/TotalMs", (t4 - t0) / 1_000_000); } } @@ -260,23 +250,12 @@ public void aim(Translation3d target) { // Profiling output for aim() if (Constants.PROFILING_ENABLED) { - long totalUs = (t5 - aimStart) / 1_000; - if (totalUs > 500) { - System.out.println( - "[Launcher.aim] v0nom=" - + (t1 - aimStart) / 1_000 - + "us baseSpeeds=" - + (t2 - t1) / 1_000 - + "us v0replan=" - + (t3 - t2) / 1_000 - + "us setPos=" - + (t4 - t3) / 1_000 - + "us rest=" - + (t5 - t4) / 1_000 - + "us total=" - + totalUs - + "us"); - } + Logger.recordOutput("Profiling/Launcher/Aim/V0NomUs", (t1 - aimStart) / 1_000); + Logger.recordOutput("Profiling/Launcher/Aim/BaseSpeedsUs", (t2 - t1) / 1_000); + Logger.recordOutput("Profiling/Launcher/Aim/V0ReplanUs", (t3 - t2) / 1_000); + Logger.recordOutput("Profiling/Launcher/Aim/SetPosUs", (t4 - t3) / 1_000); + Logger.recordOutput("Profiling/Launcher/Aim/RestUs", (t5 - t4) / 1_000); + Logger.recordOutput("Profiling/Launcher/Aim/TotalUs", (t5 - aimStart) / 1_000); } } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 2b6bafdb..7893adea 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -262,23 +262,12 @@ public void periodic() { // Profiling output if (Constants.PROFILING_ENABLED) { - long totalMs = (t5 - visionStart) / 1_000_000; - if (totalMs > 5) { - System.out.println( - "[Vision] snapshot=" - + (t1 - visionStart) / 1_000_000 - + "ms processInputs=" - + (t2 - t1) / 1_000_000 - + "ms cameraLoop=" - + (t3 - t2) / 1_000_000 - + "ms consumer=" - + (t4 - t3) / 1_000_000 - + "ms summaryLog=" - + (t5 - t4) / 1_000_000 - + "ms total=" - + totalMs - + "ms"); - } + Logger.recordOutput("Profiling/Vision/SnapshotMs", (t1 - visionStart) / 1_000_000); + Logger.recordOutput("Profiling/Vision/ProcessInputsMs", (t2 - t1) / 1_000_000); + Logger.recordOutput("Profiling/Vision/CameraLoopMs", (t3 - t2) / 1_000_000); + Logger.recordOutput("Profiling/Vision/ConsumerMs", (t4 - t3) / 1_000_000); + Logger.recordOutput("Profiling/Vision/SummaryLogMs", (t5 - t4) / 1_000_000); + Logger.recordOutput("Profiling/Vision/TotalMs", (t5 - visionStart) / 1_000_000); } }