From fc9430a9d2f6a805c3e9652457c7e91b6bfb049b Mon Sep 17 00:00:00 2001 From: josh <102478723+realjoshuau@users.noreply.github.com> Date: Sun, 22 Sep 2024 12:12:07 -0700 Subject: [PATCH] file console source + logging dmesg/messages (#51) --- src/main/java/frc/robot/Robot.java | 35 ++++- .../frc/robot/limelight/LimelightHelpers.java | 6 +- .../frc/robot/utils/FileConsoleSource.java | 139 ++++++++++++++++++ 3 files changed, 174 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc/robot/utils/FileConsoleSource.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4c84295a..36e997c4 100755 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.autos.LocalADStarAK; +import frc.robot.utils.FileConsoleSource; import frc.robot.utils.NT4PublisherNoFMS; import java.util.HashMap; import java.util.Map; @@ -36,9 +37,14 @@ * project. */ public class Robot extends LoggedRobot implements Logged { + public Robot() { + super(); + } private RobotContainer m_robotContainer; private Command m_autonomousCommand; + private FileConsoleSource dmesgSource; + private FileConsoleSource kernelMessagesSource; /** * This function is run when the robot is first started up and should be used for any @@ -49,6 +55,7 @@ public void robotInit() { Pathfinding.setPathfinder(new LocalADStarAK()); PathfindingCommand.warmupCommand().schedule(); RobotController.setBrownoutVoltage(5.6); // we ball + // if (Constants.kOverrideBrownOutVoltage) { // RobotController.setBrownoutVoltage(Constants.kOverridenBrownOutVoltage); // System.out.println( @@ -89,11 +96,15 @@ public void robotInit() { Logger.addDataReceiver(new NT4PublisherNoFMS()); // Publish data to NetworkTables new PowerDistribution( 1, PowerDistribution.ModuleType.kRev); // Enables power distribution logging + dmesgSource = new FileConsoleSource("/var/log/dmesg"); + kernelMessagesSource = new FileConsoleSource("/var/log/messages"); } else if (isSimulation()) { + // DriverStation.silenceJoystickConnectionWarning(true); Logger.addDataReceiver(new WPILOGWriter("")); Logger.addDataReceiver(new NT4Publisher()); } else { + // Unknown mode Logger.addDataReceiver(new WPILOGWriter("")); Logger.addDataReceiver(new NT4Publisher()); @@ -167,6 +178,10 @@ public void robotPeriodic() { Monologue.setFileOnly(DriverStation.isFMSAttached()); Monologue.updateAll(); } + if (dmesgSource != null && kernelMessagesSource != null && isReal()) { + Logger.recordOutput("dmesg", dmesgSource.getNewData()); + Logger.recordOutput("kernelMessages", kernelMessagesSource.getNewData()); + } m_robotContainer.periodic(Robot.defaultPeriodSecs); } @@ -207,7 +222,14 @@ public void autonomousInit() { // schedule the autonomous command (example) if (m_autonomousCommand != null) { - System.out.println("hehe auto startie " + Timer.getFPGATimestamp()); + System.out.println( + "Auto Command " + + m_autonomousCommand.getName() + + " starting at " + + Timer.getFPGATimestamp() + + " (real time: " + + Logger.getRealTimestamp() + + ")"); m_autonomousCommand.schedule(); } } @@ -216,15 +238,20 @@ public void autonomousInit() { @Override public void autonomousPeriodic() {} + @Override + public void autonomousExit() { + m_robotContainer.disableRumble(); + } + @Override public void teleopInit() { // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. - // if (m_autonomousCommand != null) { - // m_autonomousCommand.cancel(); - // } + // if (m_autonomousCommand != null) { + // m_autonomousCommand.cancel(); + // } CommandScheduler.getInstance().cancelAll(); boolean isRedAlliance = true; diff --git a/src/main/java/frc/robot/limelight/LimelightHelpers.java b/src/main/java/frc/robot/limelight/LimelightHelpers.java index 7555affd..d1ebc7ba 100644 --- a/src/main/java/frc/robot/limelight/LimelightHelpers.java +++ b/src/main/java/frc/robot/limelight/LimelightHelpers.java @@ -28,6 +28,7 @@ import java.net.MalformedURLException; import java.net.URL; import java.util.concurrent.CompletableFuture; +import org.littletonrobotics.junction.Logger; public class LimelightHelpers { @@ -437,7 +438,7 @@ public PoseEstimate( private static ObjectMapper mapper; /** Print JSON Parse time to the console in milliseconds */ - static boolean profileJSON = false; + static boolean profileJSON = true; static final String sanitizeName(String name) { if (name == "" || name == null) { @@ -987,7 +988,8 @@ public static LimelightResults getLatestResults(String limelightName) { double millis = (end - start) * .000001; results.targetingResults.latency_jsonParse = millis; if (profileJSON) { - System.out.printf("lljson: %.2f\r\n", millis); + Logger.recordOutput("lljson-" + limelightName, millis); + // System.out.printf("lljson: %.2f\r\n", millis); } return results; diff --git a/src/main/java/frc/robot/utils/FileConsoleSource.java b/src/main/java/frc/robot/utils/FileConsoleSource.java new file mode 100644 index 00000000..b20a5f39 --- /dev/null +++ b/src/main/java/frc/robot/utils/FileConsoleSource.java @@ -0,0 +1,139 @@ +// Copyright (c) 2024 FRC 3256 +// https://github.com/Team3256 +// +// Use of this source code is governed by a +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.utils; + +import edu.wpi.first.wpilibj.DriverStation; +import java.io.BufferedReader; +import java.io.FileNotFoundException; +import java.io.FileReader; +import java.io.IOException; +import java.nio.BufferOverflowException; +import java.nio.CharBuffer; +import java.util.ArrayList; +import java.util.List; +import java.util.concurrent.ArrayBlockingQueue; +import java.util.concurrent.BlockingQueue; +import org.littletonrobotics.junction.console.ConsoleSource; + +/** + * Reads file data on the RIO. Saves stdout to WPILog console. Useful for checking if CANivore is + * disconnected. (CANivore API is slow and blocking. Checking it in a periodic() loop will lag the + * robot.) + */ +public class FileConsoleSource implements ConsoleSource { + private final String filePath; + private final Thread thread; + private final String logName; + private final BlockingQueue queue = new ArrayBlockingQueue<>(100); + private final List lines = new ArrayList<>(); + + public FileConsoleSource(String filePath) { + this.filePath = filePath; + this.logName = Integer.toHexString(this.hashCode()); + thread = new Thread(this::run, "FileConsoleSource." + Integer.toHexString(this.hashCode())); + thread.setDaemon(true); + thread.start(); + } + + public FileConsoleSource(String filePath, String logName) { + this.filePath = filePath; + this.logName = logName; + thread = new Thread(this::run, "FileConsoleSource." + logName); + thread.setDaemon(true); + thread.start(); + } + + public String getLogName() { + return logName; + } + + public String getNewData() { + lines.clear(); + queue.drainTo(lines); + return String.join("\n", lines); + } + + public void close() throws Exception { + thread.interrupt(); + } + + private void run() { + // Initialize reader + CharBuffer buffer = CharBuffer.allocate(10240); + BufferedReader reader; + try { + reader = new BufferedReader(new FileReader(filePath)); + } catch (FileNotFoundException e) { + DriverStation.reportError( + "Failed to open console file \"" + filePath + "\", disabling console capture.", true); + return; + } + + while (true) { + // Read new data from console + while (true) { + int nextChar = -1; + try { + nextChar = reader.read(); + } catch (IOException e) { + DriverStation.reportError( + "Failed to read console file \"" + filePath + "\", disabling console capture.", true); + try { + reader.close(); + } catch (IOException io) { + } + return; + } + if (nextChar != -1) { + try { + buffer.put((char) nextChar); + } catch (BufferOverflowException e) { + } + } else { + // Break read loop, send complete lines to queue + break; + } + } + + // Read all complete lines + String output = null; + for (int i = buffer.position(); i > 0; i--) { + if (i < buffer.position() && buffer.get(i) == '\n') { + int originalPosition = buffer.position(); + output = new String(buffer.array(), 0, i); + buffer.rewind(); + buffer.put(buffer.array(), i + 1, buffer.limit() - i - 1); + buffer.position(originalPosition - i - 1); + break; + } + } + if (output != null) { + try { + queue.put(output); + } catch (InterruptedException e) { + try { + reader.close(); + } catch (IOException io) { + } + return; + } + } + + // Sleep to avoid spinning needlessly + try { + Thread.sleep(20); + } catch (InterruptedException e) { + try { + reader.close(); + } catch (IOException io) { + } + return; + } + } + } +}