diff --git a/src/main/java/com/team766/framework/ContextImpl.java b/src/main/java/com/team766/framework/ContextImpl.java index 2edac358..1920de88 100644 --- a/src/main/java/com/team766/framework/ContextImpl.java +++ b/src/main/java/com/team766/framework/ContextImpl.java @@ -416,7 +416,7 @@ public void end(boolean interrupted) { @Override public void execute() { ReservingCommand.enterCommand(this); - try { + try (var profileScope = Profiling.scope("Procedures/" + m_procedure.getName())) { if (m_state == State.DONE) { return; } diff --git a/src/main/java/com/team766/framework/Mechanism.java b/src/main/java/com/team766/framework/Mechanism.java index 463072c7..f84dff42 100644 --- a/src/main/java/com/team766/framework/Mechanism.java +++ b/src/main/java/com/team766/framework/Mechanism.java @@ -120,7 +120,7 @@ public final Set getReservableSubsystems() { } /* package */ final void periodicInternal() { - try { + try (var profileScope = Profiling.scope("Mechanisms/" + getName())) { publishStatus(); isRunningPeriodic = true; diff --git a/src/main/java/com/team766/framework/MultiFacetedMechanism.java b/src/main/java/com/team766/framework/MultiFacetedMechanism.java index 46f95374..1ae6daf1 100644 --- a/src/main/java/com/team766/framework/MultiFacetedMechanism.java +++ b/src/main/java/com/team766/framework/MultiFacetedMechanism.java @@ -81,20 +81,22 @@ public Category getLoggerCategory() { } /* package */ final void periodicInternal() { - for (var m : facets) { - m.periodicInternal(); - } - - try { - publishStatus(); - - isRunningPeriodic = true; - run(); - } catch (Exception ex) { - ex.printStackTrace(); - LoggerExceptionUtils.logException(ex); - } finally { - isRunningPeriodic = false; + try (var profileScope = Profiling.scope("Mechanisms/" + getName())) { + for (var m : facets) { + m.periodicInternal(); + } + + try { + publishStatus(); + + isRunningPeriodic = true; + run(); + } catch (Exception ex) { + ex.printStackTrace(); + LoggerExceptionUtils.logException(ex); + } finally { + isRunningPeriodic = false; + } } } diff --git a/src/main/java/com/team766/framework/Profiling.java b/src/main/java/com/team766/framework/Profiling.java new file mode 100644 index 00000000..437c117f --- /dev/null +++ b/src/main/java/com/team766/framework/Profiling.java @@ -0,0 +1,84 @@ +package com.team766.framework; + +import com.team766.config.ConfigFileReader; +import com.team766.library.RateLimiter; +import com.team766.library.ValueProvider; +import com.team766.logging.Category; +import com.team766.logging.Severity; +import org.littletonrobotics.junction.Logger; + +public class Profiling { + private static double getTime() { + return System.currentTimeMillis(); + } + + public static class Scope implements AutoCloseable { + private double start; + private int index; + private String name; + + private void start(String name, int index) { + this.name = name; + this.index = index; + this.start = getTime(); + } + + @Override + public void close() { + if (index != scopeIndex - 1) { + throw new IllegalStateException( + "Ended scope " + + name + + " but expected end of scope " + + scopes[scopeIndex - 1]); + } + --scopeIndex; + addSample(name, getTime() - start); + name = null; + } + } + + private static final int PROFILING_SCOPE_STACK_DEPTH = 20; + + private static final ValueProvider profilingEnabled = + ConfigFileReader.instance.getBoolean("profiling.enabled"); + private static final Scope[] scopes = new Scope[PROFILING_SCOPE_STACK_DEPTH]; + private static int scopeIndex = 0; + private static RateLimiter logWarningLimiter = new RateLimiter(1.0); + + static { + for (int i = 0; i < scopes.length; ++i) { + scopes[i] = new Scope(); + } + } + + public static Scope scope(String name) { + if (!profilingEnabled.valueOr(false)) { + return null; + } + if (scopeIndex >= scopes.length) { + if (logWarningLimiter.next()) { + com.team766.logging.Logger.get(Category.FRAMEWORK) + .logRaw( + Severity.ERROR, + "Exceeded the maximum stack depth of profiling scopes. Increase " + + "PROFILING_SCOPE_STACK_DEPTH or reduce the number of " + + "profiling scopes."); + } + return null; + } + final var scope = scopes[scopeIndex]; + scope.start(name, scopeIndex); + ++scopeIndex; + return scope; + } + + public static void addSample(String name, double duration) { + if (!profilingEnabled.valueOr(false)) { + return; + } + Logger.recordOutput("Profiling/" + name, duration); + } + + private Profiling() {} +} diff --git a/src/main/java/com/team766/framework/RuleEngine.java b/src/main/java/com/team766/framework/RuleEngine.java index f3156d48..c5b8cd88 100644 --- a/src/main/java/com/team766/framework/RuleEngine.java +++ b/src/main/java/com/team766/framework/RuleEngine.java @@ -82,102 +82,104 @@ private void sealRules() { } public final void run() { - if (!sealed) { - sealRules(); - sealed = true; - } - - Set subsystemsToUse = new HashSet<>(); - - // TODO(MF3): when creating a Procedure, check that the reservations are the same as - // what the Rule pre-computed. + try (var engineProfilingScope = Profiling.scope(getName())) { + if (!sealed) { + sealRules(); + sealed = true; + } - // evaluate each rule - ruleLoop: - for (Rule rule : rules.values()) { - try { - rule.evaluate(); - - // see if the rule is triggering - final Rule.TriggerState triggerState = rule.getCurrentTriggerState(); - if (triggerState != Rule.TriggerState.NONE) { - int priority = getPriorityForRule(rule); - - // see if there are mechanisms a potential procedure would want to reserve - Set reservations = rule.getSubsystemsToReserve(); - for (Subsystem subsystem : reservations) { - // see if any of the mechanisms higher priority rules will use would also be - // used by this lower priority rule's procedure. - if (subsystemsToUse.contains(subsystem)) { - rule.reset(ResetReason.IGNORED); - continue ruleLoop; - } - // see if a previously triggered rule is still using the mechanism - Command existingCommand = - CommandScheduler.getInstance().requiring(subsystem); - if (existingCommand != null) { - // look up the rule - Rule existingRule = getRuleForTriggeredProcedure(existingCommand); - if (existingRule != null) { - // look up the priority - int existingPriority = getPriorityForRule(existingRule); - if (existingPriority < priority /* less is more */) { - // existing rule takes priority. - // don't proceed with this new rule. - rule.reset(ResetReason.IGNORED); - continue ruleLoop; - } else if (rule != existingRule) { - // new rule takes priority - // reset existing rule - existingRule.reset(ResetReason.PREEMPTED); + Set subsystemsToUse = new HashSet<>(); + + // TODO(MF3): when creating a Procedure, check that the reservations are the same as + // what the Rule pre-computed. + + // evaluate each rule + ruleLoop: + for (Rule rule : rules.values()) { + try (var rulleProfilingScope = Profiling.scope("Rules/" + rule.getName())) { + rule.evaluate(); + + // see if the rule is triggering + final Rule.TriggerState triggerState = rule.getCurrentTriggerState(); + if (triggerState != Rule.TriggerState.NONE) { + int priority = getPriorityForRule(rule); + + // see if there are mechanisms a potential procedure would want to reserve + Set reservations = rule.getSubsystemsToReserve(); + for (Subsystem subsystem : reservations) { + // see if any of the mechanisms higher priority rules would also be + // used by this lower priority rule's procedure. + if (subsystemsToUse.contains(subsystem)) { + rule.reset(ResetReason.IGNORED); + continue ruleLoop; + } + // see if a previously triggered rule is still using the mechanism + Command existingCommand = + CommandScheduler.getInstance().requiring(subsystem); + if (existingCommand != null) { + // look up the rule + Rule existingRule = getRuleForTriggeredProcedure(existingCommand); + if (existingRule != null) { + // look up the priority + int existingPriority = getPriorityForRule(existingRule); + if (existingPriority < priority /* less is more */) { + // existing rule takes priority. + // don't proceed with this new rule. + rule.reset(ResetReason.IGNORED); + continue ruleLoop; + } else if (rule != existingRule) { + // new rule takes priority + // reset existing rule + existingRule.reset(ResetReason.PREEMPTED); + } } } } - } - // we're good to proceed - if (triggerState == Rule.TriggerState.FINISHED - && rule.getCancellationOnFinish() - == Rule.Cancellation.CANCEL_NEWLY_ACTION) { - var newlyCommand = - ruleMap.inverse() - .get(new RuleAction(rule, Rule.TriggerState.NEWLY)); - if (newlyCommand != null) { - newlyCommand.cancel(); + // we're good to proceed + if (triggerState == Rule.TriggerState.FINISHED + && rule.getCancellationOnFinish() + == Rule.Cancellation.CANCEL_NEWLY_ACTION) { + var newlyCommand = + ruleMap.inverse() + .get(new RuleAction(rule, Rule.TriggerState.NEWLY)); + if (newlyCommand != null) { + newlyCommand.cancel(); + } } - } - Procedure procedure = rule.getProcedureToRun(); - if (procedure == null) { - continue; + Procedure procedure = rule.getProcedureToRun(); + if (procedure == null) { + continue; + } + log( + Severity.INFO, + "Rule " + + rule.getName() + + " triggered (" + + rule.getCurrentTriggerState() + + "). Running Procedure " + + procedure.getName() + + " with reservations " + + reservations); + + // TODO(MF3): check that the reservations have not changed + Command command = procedure.createCommandToRunProcedure(); + subsystemsToUse.addAll(reservations); + ruleMap.forcePut(command, new RuleAction(rule, triggerState)); + command.schedule(); } + } catch (Exception ex) { log( - Severity.INFO, - "Rule " - + rule.getName() - + " triggered (" - + rule.getCurrentTriggerState() - + "). Running Procedure " - + procedure.getName() - + " with reservations " - + reservations); - - // TODO(MF3): check that the reservations have not changed - Command command = procedure.createCommandToRunProcedure(); - subsystemsToUse.addAll(reservations); - ruleMap.forcePut(command, new RuleAction(rule, triggerState)); - command.schedule(); + Severity.ERROR, + "Exception caught while trying to run(): " + + LoggerExceptionUtils.exceptionToString(ex)); } - } catch (Exception ex) { - log( - Severity.ERROR, - "Exception caught while trying to run(): " - + LoggerExceptionUtils.exceptionToString(ex)); } - } - for (Rule rule : rules.values()) { - rule.flushLog(); + for (Rule rule : rules.values()) { + rule.flushLog(); + } } } } diff --git a/src/main/java/com/team766/hal/wpilib/RobotMain.java b/src/main/java/com/team766/hal/wpilib/RobotMain.java index 377537a5..966258a5 100755 --- a/src/main/java/com/team766/hal/wpilib/RobotMain.java +++ b/src/main/java/com/team766/hal/wpilib/RobotMain.java @@ -2,6 +2,7 @@ import com.team766.BuildConstants; import com.team766.config.ConfigFileReader; +import com.team766.framework.Profiling; import com.team766.hal.CanivPoller; import com.team766.hal.GenericRobotMain; import com.team766.hal.RobotProvider; @@ -142,6 +143,16 @@ public void robotInit() { } } + private Profiling.Scope robotLoopScope = null; + + @Override + public void robotPeriodic() { + if (robotLoopScope != null) { + robotLoopScope.close(); + } + robotLoopScope = Profiling.scope("RobotMain"); + } + @Override public void disabledInit() { try {