diff --git a/.gitignore b/.gitignore index 214ea0c9..802d3e91 100644 --- a/.gitignore +++ b/.gitignore @@ -183,3 +183,6 @@ simgui-ds.json simgui.json !src/main/deploy/photon-configs/*.zip + +/*.wpilog +/*-profile.txt diff --git a/build.gradle b/build.gradle index efa08734..30dba506 100644 --- a/build.gradle +++ b/build.gradle @@ -104,6 +104,7 @@ dependencies { implementation 'org.apache.httpcomponents:httpclient:4.5.14' implementation 'org.apache.httpcomponents:httpmime:4.5.14' + implementation 'it.unimi.dsi:fastutil:8.5.14' } // Simulation configuration (e.g. environment variables). diff --git a/src/main/java/frc/lib/profiling/EmptyProfiler.java b/src/main/java/frc/lib/profiling/EmptyProfiler.java new file mode 100644 index 00000000..7246bc7f --- /dev/null +++ b/src/main/java/frc/lib/profiling/EmptyProfiler.java @@ -0,0 +1,40 @@ +package frc.lib.profiling; + +/** + * This logger explicitly does nothing. Because there is no variance in doing nothing, a single + * instance is provided as {@link #INSTANCE}. + */ +public final class EmptyProfiler implements Profiler { + + /** + * The sole instance of {@link EmptyProfiler}. + */ + public static final EmptyProfiler INSTANCE = new EmptyProfiler(); + + private EmptyProfiler() {} + + @Override + public void save() { + + } + + @Override + public void startTick() { + + } + + @Override + public void endTick() { + + } + + @Override + public void push(String location) { + + } + + @Override + public void pop() { + + } +} diff --git a/src/main/java/frc/lib/profiling/LoggingProfiler.java b/src/main/java/frc/lib/profiling/LoggingProfiler.java new file mode 100644 index 00000000..ba1adfef --- /dev/null +++ b/src/main/java/frc/lib/profiling/LoggingProfiler.java @@ -0,0 +1,195 @@ +package frc.lib.profiling; + +import java.io.FileOutputStream; +import java.io.IOException; +import java.lang.reflect.Field; +import java.time.LocalDateTime; +import java.time.format.DateTimeFormatter; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.function.LongSupplier; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.ReceiverThread; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import it.unimi.dsi.fastutil.longs.LongArrayList; +import it.unimi.dsi.fastutil.longs.LongList; + +/** + * This logger saves runtime performance metrics to memory. These metrics are subsequently written + * to a file when {@link #save() save} is called. + */ +public final class LoggingProfiler implements Profiler { + private static final char SPLIT_CHAR = '\u001e'; + + private final List path = new ArrayList<>(); + private final LongList timeList = new LongArrayList(); + private final Map locationInfos = new HashMap<>(); + private final LongSupplier timeGetter; + private final double timeDivisor; + private final ProfileLoggingFormat loggingFormat; + private boolean tickStarted; + private String fullPath = ""; + private LoggingProfiler.LocatedInfo currentInfo; + + /** + * @param filePath file path to write when {@link #save() save} is called. + * @param timeGetter a supplier for the current time. + * @param timeDivisor a conversion factor turning the units of {@code timeGetter} to the units + * output to a file. + * @param loggingFormat file format to use when {@link #save() save} is called. + */ + public LoggingProfiler(LongSupplier timeGetter, double timeDivisor, + ProfileLoggingFormat loggingFormat) { + this.timeGetter = timeGetter; + this.timeDivisor = timeDivisor; + this.loggingFormat = loggingFormat; + } + + @Override + public void save() { + try { + String filePath = null; + + Field dataReceiversField = ReceiverThread.class.getDeclaredField("dataReceivers"); + dataReceiversField.setAccessible(true); + Field receiverThreadField = Logger.class.getDeclaredField("receiverThread"); + receiverThreadField.setAccessible(true); + Field logDateField = WPILOGWriter.class.getDeclaredField("logDate"); + logDateField.setAccessible(true); + Field randomIdentifierField = WPILOGWriter.class.getDeclaredField("randomIdentifier"); + randomIdentifierField.setAccessible(true); + Field timeFormatterField = WPILOGWriter.class.getDeclaredField("timeFormatter"); + timeFormatterField.setAccessible(true); + Field logMatchTextField = WPILOGWriter.class.getDeclaredField("logMatchText"); + logMatchTextField.setAccessible(true); + + Object dataReceivers = dataReceiversField.get(receiverThreadField.get(null)); + if (dataReceivers instanceof List dataReceiversList) { + for (Object dataReceiver : dataReceiversList) { + if (dataReceiver instanceof WPILOGWriter dataWriter) { + StringBuilder newFilenameBuilder = new StringBuilder(); + newFilenameBuilder.append("Log_"); + LocalDateTime logDate = (LocalDateTime) logDateField.get(dataWriter); + if (logDate == null) { + Object randomIdentifier = randomIdentifierField.get(dataWriter); + newFilenameBuilder.append(randomIdentifier); + } else { + DateTimeFormatter timeFormatter = + (DateTimeFormatter) timeFormatterField.get(dataWriter); + newFilenameBuilder.append(timeFormatter.format(logDate)); + } + Object logMatchText = logMatchTextField.get(dataWriter); + if (logMatchText != null) { + newFilenameBuilder.append("_"); + newFilenameBuilder.append(logMatchText); + } + newFilenameBuilder.append("-profile.txt"); + filePath = newFilenameBuilder.toString(); + } + } + } + try (var outStream = new FileOutputStream(filePath)) { + loggingFormat.begin(outStream); + for (var entry : locationInfos.entrySet().stream().sorted((a, b) -> loggingFormat + .compare(a.getKey(), a.getValue(), b.getKey(), b.getValue())).toList()) { + var info = entry.getValue(); + var name = entry.getKey().replace(SPLIT_CHAR, '.'); + loggingFormat.write(name, info, timeDivisor, outStream); + } + loggingFormat.end(outStream); + } + } catch (IOException | NoSuchFieldException | SecurityException | IllegalArgumentException + | IllegalAccessException e) { + throw new RuntimeException(e); + } + } + + @Override + public void startTick() { + if (this.tickStarted) { + throw new RuntimeException("Profiler tick already started. Missing endTick()?"); + } else { + this.tickStarted = true; + this.fullPath = ""; + this.path.clear(); + this.push("root"); + } + } + + @Override + public void endTick() { + if (!this.tickStarted) { + throw new RuntimeException("Profiler tick already ended. Missing startTick()?"); + } else { + this.pop(); + this.tickStarted = false; + if (!this.fullPath.isEmpty()) { + throw new RuntimeException( + "Profiler tick ended before path was fully popped. Mismatched push/pop?"); + } + } + } + + @Override + public void push(String location) { + if (!this.tickStarted) { + throw new RuntimeException("Cannot push '" + location + + "' to the profiler if profiler tick hasn't started. Missing startTick()?"); + } else { + if (!this.fullPath.isEmpty()) { + this.fullPath = this.fullPath + SPLIT_CHAR; + } + this.fullPath += location; + this.path.add(this.fullPath); + this.timeList.add(timeGetter.getAsLong()); + this.currentInfo = null; + } + } + + @Override + public void pop() { + if (!this.tickStarted) { + throw new RuntimeException( + "Cannot pop from profiler if profiler tick hasn't started. Missing startTick()?"); + } else if (this.timeList.isEmpty()) { + throw new RuntimeException( + "Tried to pop one too many times! Mismatched push() and pop()?"); + } else { + this.path.remove(this.path.size() - 1); + + long currentTime = timeGetter.getAsLong(); + long startTime = this.timeList.removeLong(this.timeList.size() - 1); + long timeSpan = currentTime - startTime; + + LocatedInfo locatedInfo = this.getCurrentInfo(); + + locatedInfo.totalTime += timeSpan; + locatedInfo.visits++; + locatedInfo.maxTime = Math.max(locatedInfo.maxTime, timeSpan); + locatedInfo.minTime = Math.min(locatedInfo.minTime, timeSpan); + this.fullPath = this.path.isEmpty() ? "" : this.path.get(this.path.size() - 1); + this.currentInfo = null; + } + } + + private LocatedInfo getCurrentInfo() { + if (this.currentInfo == null) { + this.currentInfo = + this.locationInfos.computeIfAbsent(this.fullPath, _k -> new LocatedInfo()); + } + return this.currentInfo; + } + + /** + * Performance metrics for a given state of the profile stack. + */ + public static class LocatedInfo { + long maxTime = Long.MIN_VALUE; + long minTime = Long.MAX_VALUE; + long totalTime; + long visits; + } + +} diff --git a/src/main/java/frc/lib/profiling/ProfileLoggingFormat.java b/src/main/java/frc/lib/profiling/ProfileLoggingFormat.java new file mode 100755 index 00000000..8bd5cf6e --- /dev/null +++ b/src/main/java/frc/lib/profiling/ProfileLoggingFormat.java @@ -0,0 +1,31 @@ +package frc.lib.profiling; + +import java.io.OutputStream; +import frc.lib.profiling.LoggingProfiler.LocatedInfo; + +/** + * Describes how a profile is written to a file. + */ +public interface ProfileLoggingFormat { + + /** + * Comparison function used for sorting profile information. + */ + int compare(String leftName, LocatedInfo leftInfo, String rightName, LocatedInfo rightInfo); + + /** + * Write a profile to the file specified in {@code outStream}. + */ + void write(String name, LocatedInfo info, double timeDivisor, OutputStream outStream); + + /** + * Write information before seeing any profiles. + */ + void begin(OutputStream outputStream); + + /** + * Write information after seeing all profiles. + */ + void end(OutputStream outputStream); + +} diff --git a/src/main/java/frc/lib/profiling/Profiler.java b/src/main/java/frc/lib/profiling/Profiler.java new file mode 100644 index 00000000..f8c4d4b5 --- /dev/null +++ b/src/main/java/frc/lib/profiling/Profiler.java @@ -0,0 +1,87 @@ +package frc.lib.profiling; + +/** + * Interface supporting profiling calls for measuring and saving runtime performance. + * + * Operates on the idea of a "profile stack", which is just a stack of names with associated + * profiling metrics. For instance, the following code + * + * 
+ * {@code
+ * Profiler profiler = someProfiler();
+ * profiler.startTick();
+ * profiler.push("Hello")
+ * someExpensiveFunction();
+ * profiler.swap("World")
+ * someLessExpensiveFunction();
+ * profiler.pop();
+ * profiler.endTick();
+ * }
+ *
+ * + * will produce a profile with three sections: + * + * + * A call to {@link #save() profiler.save()} would output the performance metrics of these sections + * in the case of a {@link LoggingProfiler}. + */ +public sealed interface Profiler permits EmptyProfiler, LoggingProfiler, ValidatingProfiler { + /** + * Called at the top of the main loop. Indicates the profiler is at "root" and records the start + * time. Within the main loop, no meaningful work should occur before this call. + * + * @throws RuntimeException if called twice without a call to {@link #endTick() endTick} in + * between. + */ + void startTick(); + + /** + * Called at the bottom of the main loop. Indicates the profiler's "root" is finished, and its + * time usage metrics are updated. Within the main loop, no meaningful work should occur after + * this call. + * + * @throws RuntimeException if the profile stack is not empty or {@link #startTick() startTick} + * has not been called. + */ + void endTick(); + + /** + * The profile stack is pushed with the {@code location} as its top. Must be accompanied by a + * call to {@link #pop() pop}. + * + * @throws RuntimeException if {@link #startTick() startTick} hasn't been called yet. + */ + void push(String location); + + /** + * The profile stack is popped. Must be preceded (at some point) by a call to + * {@link #push(String) push}. + * + * @throws RuntimeException if {@link #startTick() startTick} hasn't been called yet or the + * profile stack is empty. + */ + void pop(); + + /** + * The top of the profile stack is replaced with {@code location}. This is equivalent to a call + * to {@link #pop() pop} followed immediately by a call to {@link #push(String) push}. + * + * @throws RuntimeException if the profile stack is empty. + */ + default void swap(String location) { + pop(); + push(location); + } + + /** + * Write profile data to a file. + */ + void save(); +} diff --git a/src/main/java/frc/lib/profiling/TextProfileLoggingFormat.java b/src/main/java/frc/lib/profiling/TextProfileLoggingFormat.java new file mode 100755 index 00000000..a18b7c16 --- /dev/null +++ b/src/main/java/frc/lib/profiling/TextProfileLoggingFormat.java @@ -0,0 +1,57 @@ +package frc.lib.profiling; + +import java.io.IOException; +import java.io.OutputStream; +import frc.lib.profiling.LoggingProfiler.LocatedInfo; + +/** + * Logging format that writes to a human-readable text file. Profile sections are sorted from by + * longest total time (from most time to least time). Because there is no variance, a single + * instance is provided as {@link #INSTANCE}. + */ +public class TextProfileLoggingFormat implements ProfileLoggingFormat { + private TextProfileLoggingFormat() {} + + /** + * The sole instance of {@link TextProfileLoggingFormat}. + */ + public static final TextProfileLoggingFormat INSTANCE = new TextProfileLoggingFormat(); + + @Override + public int compare(String leftName, LocatedInfo leftInfo, String rightName, + LocatedInfo rightInfo) { + return -Long.compare(leftInfo.totalTime, rightInfo.totalTime); + } + + @Override + public void write(String name, LocatedInfo info, double timeDivisor, OutputStream outStream) { + try { + outStream.write((name + "\n").getBytes()); + outStream.write((" visitCount: " + info.visits + "\n").getBytes()); + outStream.write((" totalTime: " + info.totalTime / timeDivisor + "\n").getBytes()); + outStream.write((" maxTime: " + info.maxTime / timeDivisor + "\n").getBytes()); + outStream.write((" minTime: " + info.minTime / timeDivisor + "\n").getBytes()); + outStream.write( + (" avgTime: " + info.totalTime / info.visits / timeDivisor + "\n").getBytes()); + } catch (IOException e) { + throw new RuntimeException(e); + } + } + + @Override + public void begin(OutputStream outputStream) { + try { + outputStream.write(("Performance Log\n" + // + "================================================================================\n") + .getBytes()); + } catch (IOException e) { + throw new RuntimeException(e); + } + } + + @Override + public void end(OutputStream outputStream) { + + } + +} diff --git a/src/main/java/frc/lib/profiling/ValidatingProfiler.java b/src/main/java/frc/lib/profiling/ValidatingProfiler.java new file mode 100644 index 00000000..0755dcd7 --- /dev/null +++ b/src/main/java/frc/lib/profiling/ValidatingProfiler.java @@ -0,0 +1,78 @@ +package frc.lib.profiling; + +/** + * This logger only performs validation (i.e. throws if {@link #pop() pop} is called with an empty + * stack). Otherwise, does nothing. Because there is no variance in doing nothing, a single instance + * is provided as {@link #INSTANCE}. + */ +public final class ValidatingProfiler implements Profiler { + + /** + * The sole instance of {@link ValidatingProfiler}. + */ + public static final ValidatingProfiler INSTANCE = new ValidatingProfiler(); + + private ValidatingProfiler() {} + + private boolean tickStarted = false; + private int pathLen = 0; + private boolean fullPathEmpty = true; + + @Override + public void save() { + + } + + @Override + public void startTick() { + if (this.tickStarted) { + throw new RuntimeException("Profiler tick already started. Missing endTick()?"); + } else { + tickStarted = true; + pathLen = 0; + fullPathEmpty = true; + this.push("root"); + } + } + + @Override + public void endTick() { + if (!this.tickStarted) { + throw new RuntimeException("Profiler tick already ended. Missing startTick()?"); + } else { + this.pop(); + this.tickStarted = false; + if (!this.fullPathEmpty) { + throw new RuntimeException( + "Profiler tick ended before path was fully popped. Mismatched push/pop?"); + } + } + } + + @Override + public void push(String location) { + if (!this.tickStarted) { + throw new RuntimeException("Cannot push '" + location + + "' to the profiler if profiler tick hasn't started. Missing startTick()?"); + } else { + fullPathEmpty = false; + pathLen += 1; + } + } + + @Override + public void pop() { + if (!this.tickStarted) { + throw new RuntimeException( + "Cannot pop from profiler if profiler tick hasn't started. Missing startTick()?"); + } else if (pathLen == 0) { + throw new RuntimeException( + "Tried to pop one too many times! Mismatched push() and pop()?"); + } else { + pathLen -= 1; + if (pathLen == 0) { + fullPathEmpty = true; + } + } + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a515f51b..919679fd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -14,11 +14,16 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.lib.profiling.EmptyProfiler; +import frc.lib.profiling.LoggingProfiler; +import frc.lib.profiling.Profiler; +import frc.lib.profiling.TextProfileLoggingFormat; /** * Runs tasks on Roborio in this file. @@ -27,8 +32,10 @@ public class Robot extends LoggedRobot { private RobotContainer robotContainer; private Command autoChooser; + public static Profiler profiler; + /** - * Robnot Run type + * Robot Run type */ public static enum RobotRunType { /** Real Robot. */ @@ -43,14 +50,9 @@ public static enum RobotRunType { public RobotRunType robotRunType = RobotRunType.kReal; private Timer gcTimer = new Timer(); - // private Ultrasonic ultrasonic = new Ultrasonic(); - /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ + /** Set up logging, profiling, and robotContainer. */ @SuppressWarnings("resource") - @Override - public void robotInit() { + public Robot() { // Record metadata Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); @@ -80,27 +82,35 @@ public void robotInit() { } else { String logPath = findReplayLog(); if (logPath == null) { + Logger.addDataReceiver( + new WPILOGWriter(Filesystem.getOperatingDirectory().getAbsolutePath())); Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables setUseTiming(true); robotRunType = RobotRunType.kSimulation; } else { - // (or prompt the user) Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log Logger .addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log setUseTiming(false); // Run as fast as possible robotRunType = RobotRunType.kReplay; - } } + Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values + switch (robotRunType) { + case kReal -> profiler = new LoggingProfiler(() -> Logger.getRealTimestamp(), 1000000.0, + TextProfileLoggingFormat.INSTANCE); + case kReplay -> profiler = EmptyProfiler.INSTANCE; + case kSimulation -> profiler = new LoggingProfiler(() -> Logger.getRealTimestamp(), + 1000000.0, TextProfileLoggingFormat.INSTANCE); + } // Logger.disableDeterministicTimestamps() // See "Deterministic Timestamps" in the // "Understanding Data Flow" page - Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. robotContainer = new RobotContainer(robotRunType); + profiler.startTick(); } /** @@ -114,6 +124,10 @@ public void robotInit() { @Override public void robotPeriodic() { + profiler.endTick(); + profiler.startTick(); + profiler.push("robotPeriodic()"); + profiler.push("draw_state_to_shuffleboard"); robotContainer.operatorState.setString(OperatorState.getCurrentState().displayName); robotContainer.operatorManualMode.setBoolean(OperatorState.manualModeEnabled()); robotContainer.matchTime.setDouble(Timer.getMatchTime()); @@ -125,14 +139,22 @@ public void robotPeriodic() { // subsystem periodic() methods. This must be called from the robot's periodic block in // order for // anything in the Command-based framework to work. + profiler.swap("command_scheduler"); CommandScheduler.getInstance().run(); + profiler.swap("manual-gc"); if (gcTimer.advanceIfElapsed(5)) { System.gc(); } + profiler.pop(); + profiler.pop(); } @Override - public void disabledInit() {} + public void disabledInit() { + profiler.endTick(); + profiler.save(); + profiler.startTick(); + } @Override public void disabledPeriodic() {} @@ -142,8 +164,10 @@ public void disabledPeriodic() {} */ @Override public void autonomousInit() { + profiler.push("autonomousInit()"); inAuto = true; OperatorState.disableManualMode(); + robotContainer.getAutonomousCommand().schedule(); autoChooser = robotContainer.getAutonomousCommand(); @@ -151,6 +175,7 @@ public void autonomousInit() { if (autoChooser != null) { autoChooser.schedule(); } + profiler.pop(); } /** This function is called periodically during autonomous. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2bdbc81c..5bd99434 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,7 +21,6 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.lib.util.FieldConstants; import frc.lib.util.photon.PhotonCameraWrapper; -import frc.lib.util.photon.PhotonReal; import frc.robot.Robot.RobotRunType; import frc.robot.autos.P123; import frc.robot.autos.P321; @@ -131,18 +130,19 @@ public RobotContainer(RobotRunType runtimeType) { * Camera Order: 0 - Front Left 1 - Front RIght 2 - Back Left 3 - Back Right */ new PhotonCameraWrapper[] { - new PhotonCameraWrapper( - new PhotonReal(Constants.CameraConstants.FrontLeftFacingCamera.CAMERA_NAME, - Constants.CameraConstants.FrontLeftFacingCamera.CAMERA_IP), - Constants.CameraConstants.FrontLeftFacingCamera.KCAMERA_TO_ROBOT), - new PhotonCameraWrapper( - new PhotonReal(Constants.CameraConstants.FrontRightFacingCamera.CAMERA_NAME, - Constants.CameraConstants.FrontRightFacingCamera.CAMERA_IP), - Constants.CameraConstants.FrontRightFacingCamera.KCAMERA_TO_ROBOT), - new PhotonCameraWrapper( - new PhotonReal(Constants.CameraConstants.BackLeftFacingCamera.CAMERA_NAME, - Constants.CameraConstants.BackLeftFacingCamera.CAMERA_IP), - Constants.CameraConstants.BackLeftFacingCamera.KCAMERA_TO_ROBOT)}; + // new PhotonCameraWrapper( + // new PhotonReal(Constants.CameraConstants.FrontLeftFacingCamera.CAMERA_NAME, + // Constants.CameraConstants.FrontLeftFacingCamera.CAMERA_IP), + // Constants.CameraConstants.FrontLeftFacingCamera.KCAMERA_TO_ROBOT), + // new PhotonCameraWrapper( + // new PhotonReal(Constants.CameraConstants.FrontRightFacingCamera.CAMERA_NAME, + // Constants.CameraConstants.FrontRightFacingCamera.CAMERA_IP), + // Constants.CameraConstants.FrontRightFacingCamera.KCAMERA_TO_ROBOT), + // new PhotonCameraWrapper( + // new PhotonReal(Constants.CameraConstants.BackLeftFacingCamera.CAMERA_NAME, + // Constants.CameraConstants.BackLeftFacingCamera.CAMERA_IP), + // Constants.CameraConstants.BackLeftFacingCamera.KCAMERA_TO_ROBOT) + }; // new PhotonCameraWrapper( // new PhotonReal(Constants.CameraConstants.BackRightFacingCamera.CAMERA_NAME), // Constants.CameraConstants.BackRightFacingCamera.KCAMERA_TO_ROBOT)}; diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index 2555237f..dad2a806 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; +import frc.robot.Robot; import frc.robot.subsystems.swerve.Swerve; /** @@ -49,6 +50,7 @@ public TeleopSwerve(Swerve swerveDrive, CommandXboxController controller, boolea @Override public void execute() { + Robot.profiler.push("teleop_swerve"); double yaxis = -controller.getLeftY() * speedMultiplier; double xaxis = -controller.getLeftX() * speedMultiplier; double raxis = -controller.getRightX() * speedMultiplier; @@ -67,5 +69,6 @@ public void execute() { new Translation2d(yaxis, xaxis).times(Constants.Swerve.maxSpeed); double rotation = raxis * Constants.Swerve.maxAngularVelocity; swerveDrive.drive(translation, rotation, fieldRelative, openLoop); + Robot.profiler.pop(); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java index ca12a8e0..a8bed02a 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java @@ -27,6 +27,7 @@ import frc.lib.util.photon.PhotonCameraWrapper; import frc.lib.util.swerve.SwerveModule; import frc.robot.Constants; +import frc.robot.Robot; import frc.robot.RobotContainer; /** @@ -105,12 +106,14 @@ public Swerve(SwerveIO swerveIO, PhotonCameraWrapper[] cameras) { */ public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) { + Robot.profiler.push("swerve.drive()"); ChassisSpeeds chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(translation.getX(), translation.getY(), rotation, getFieldRelativeHeading()) : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); setModuleStates(chassisSpeeds); + Robot.profiler.pop(); } /** @@ -244,17 +247,23 @@ public void resetPvInitialization() { @Override public void periodic() { + Robot.profiler.push("swerve_periodic"); + Robot.profiler.push("update_inputs"); swerveIO.updateInputs(inputs); + Robot.profiler.swap("update_swerve_mods"); for (var mod : swerveMods) { mod.periodic(); } + Robot.profiler.swap("update_swerve_odometry"); swerveOdometry.update(getGyroYaw(), getModulePositions()); + Robot.profiler.swap("process_inputs"); Logger.processInputs("Swerve", inputs); + Robot.profiler.swap("process_cameras"); for (int i = 0; i < cameras.length; i++) { cameras[i].periodic(); cameraSeesTarget[i] = cameras[i].seesTarget(); } - + Robot.profiler.swap("do_camera_stuff"); Logger.recordOutput("/Swerve/hasInitialized", hasInitialized); if (!hasInitialized && !DriverStation.isAutonomous()) { @@ -281,6 +290,7 @@ public void periodic() { } } } + Robot.profiler.swap("update_shuffleboard"); field.setRobotPose(getPose()); aprilTagTarget @@ -292,6 +302,8 @@ public void periodic() { SmartDashboard.putBoolean("Has Initialized", hasInitialized); SmartDashboard.putNumber("Gyro Yaw", getGyroYaw().getDegrees()); + Robot.profiler.pop(); + Robot.profiler.pop(); } /** diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 8c565032..0e80a16c 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.8", + "version": "v2024.3.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.8", + "version": "v2024.3.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.8", + "version": "v2024.3.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.8" + "version": "v2024.3.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.8" + "version": "v2024.3.1" } ] } \ No newline at end of file