From e636f02e470a5cd480c7a060c578fdcba3ad9ed8 Mon Sep 17 00:00:00 2001
From: Bradley Bickford <bradleybickford@gmail.com>
Date: Sat, 8 Mar 2025 22:21:56 -0500
Subject: [PATCH] Adding some basic logging

---
 src/main/java/frc/robot/Robot.java            | 26 +++++++++++++++++++
 .../frc/robot/constants/LoggingConstants.java |  7 +++++
 .../subsystems/DifferentialDrivetrain.java    |  8 ++++++
 3 files changed, 41 insertions(+)
 create mode 100644 src/main/java/frc/robot/constants/LoggingConstants.java

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index be21731..619aff3 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,16 +4,42 @@
 
 package frc.robot;
 
+import edu.wpi.first.epilogue.Epilogue;
+import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.epilogue.logging.FileBackend;
+import edu.wpi.first.epilogue.logging.errors.ErrorHandler;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.constants.LoggingConstants;
 
+@Logged
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
 
   private final RobotContainer m_robotContainer;
 
   public Robot() {
+    DataLogManager.start();
+    Epilogue.configure(config -> {
+      // Log only to disk, instead of the default NetworkTables logging
+      // Note that this means data cannot be analyzed in realtime by a dashboard
+      config.backend = new FileBackend(DataLogManager.getLog());
+
+      if (isSimulation()) {
+        // If running in simulation, then we'd want to re-throw any errors that
+        // occur so we can debug and fix them!
+        config.errorHandler = ErrorHandler.crashOnError();
+      }
+
+      // Only log critical information instead of the default DEBUG level.
+      // This can be helpful in a pinch to reduce network bandwidth or log file size
+      // while still logging important information.
+      config.minimumImportance = LoggingConstants.kImportance;
+    });
+    Epilogue.bind(this);
+    
     m_robotContainer = new RobotContainer();
   }
 
diff --git a/src/main/java/frc/robot/constants/LoggingConstants.java b/src/main/java/frc/robot/constants/LoggingConstants.java
new file mode 100644
index 0000000..e5d283d
--- /dev/null
+++ b/src/main/java/frc/robot/constants/LoggingConstants.java
@@ -0,0 +1,7 @@
+package frc.robot.constants;
+
+import edu.wpi.first.epilogue.Logged.Importance;
+
+public class LoggingConstants {
+    public static final Importance kImportance = Importance.DEBUG;
+}
diff --git a/src/main/java/frc/robot/subsystems/DifferentialDrivetrain.java b/src/main/java/frc/robot/subsystems/DifferentialDrivetrain.java
index 1753a2e..7442a66 100644
--- a/src/main/java/frc/robot/subsystems/DifferentialDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/DifferentialDrivetrain.java
@@ -1,5 +1,7 @@
 package frc.robot.subsystems;
 
+import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.epilogue.Logged.Importance;
 import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -94,6 +96,7 @@ public class DifferentialDrivetrain extends SubsystemBase implements IDrivetrain
     }
 
     @Override
+    @Logged(name = "DD Pose", importance = Importance.INFO)
     public Pose2d getPose() {
         return poseEstimator.getEstimatedPosition();
     }
@@ -185,22 +188,27 @@ public class DifferentialDrivetrain extends SubsystemBase implements IDrivetrain
         });
     }
 
+    @Logged(name = "DD Gyro Angle", importance = Importance.DEBUG)
     public double getGyroAngle() {
         return gyro.getAngle() * (DifferentialDriveConstants.kInvertGyro ? -1 : 1);
     }
 
+    @Logged(name = "DD Left Encoder Meters", importance = Importance.DEBUG)
     public double getLeftEncoderMeters() {
         return leftEncoder.getDistance() * (DifferentialDriveConstants.kInvertEncoders ? -1 : 1);
     }
 
+    @Logged(name = "DD Right Encoder Meters", importance = Importance.DEBUG)
     public double getRightEncoderMeters() {
         return rightEncoder.getDistance() * (DifferentialDriveConstants.kInvertEncoders ? -1 : 1);
     }
 
+    @Logged(name = "DD Left Encoder Meters Per Second", importance = Importance.DEBUG)
     public double getLeftEncoderMetersPerSecond() {
         return leftEncoder.getRate() * (DifferentialDriveConstants.kInvertEncoders ? -1 : 1);
     }
 
+    @Logged(name = "DD Right Encoder Meters Per Second", importance = Importance.DEBUG)
     public double getRightEncoderMetersPerSecond() {
         return rightEncoder.getRate() * (DifferentialDriveConstants.kInvertEncoders ? -1 : 1);
     }