The great code restructuring

src/main/java/frc4388/robot/Robot.java

@@ -11,24 +11,31 @@ import java.util.ArrayList;
 import java.util.Base64;
 import java.util.List;
 
+// Advantagekit
+import org.littletonrobotics.junction.LogFileUtil;
+import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.wpilog.WPILOGReader;
+import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+
 import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.CANBus.CANBusStatus;
 
-import edu.wpi.first.util.sendable.Sendable;
-import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import frc4388.utility.CanDevice;
+import frc4388.robot.constants.BuildConstants;
+import frc4388.robot.constants.Constants;
+import frc4388.robot.constants.Constants.SimConstants;
 import frc4388.utility.DeferredBlock;
 import frc4388.utility.DeferredBlockMulti;
-import frc4388.utility.RobotTime;
-import frc4388.utility.Status;
-import frc4388.utility.Subsystem;
 import frc4388.utility.Trim;
-import frc4388.utility.Status.Report;
-import frc4388.utility.Status.ReportLevel;
+import frc4388.utility.compute.RobotTime;
+import frc4388.utility.status.CanDevice;
+import frc4388.utility.status.Status;
+import frc4388.utility.status.Subsystem;
+import frc4388.utility.status.Status.Report;
+import frc4388.utility.status.Status.ReportLevel;
 //import frc4388.robot.subsystems.LED;
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -37,7 +44,7 @@ import frc4388.utility.Status.ReportLevel;
  * creating this project, you must also update the build.gradle file in the
  * project.
  */
-public class Robot extends TimedRobot {
+public class Robot extends LoggedRobot {
   Command m_autonomousCommand;
 
   private RobotTime m_robotTime = RobotTime.getInstance();
@@ -55,7 +62,7 @@ public class Robot extends TimedRobot {
     m_robotContainer = new RobotContainer();
 
 
-
+    // Create a shuffleboard update thread, that will periodically update the values on shuffleboard
     new Thread() {
       public void run() {
         try{
@@ -69,7 +76,7 @@ public class Robot extends TimedRobot {
           
         }
         }catch(Exception e){
-          e.printStackTrace();
+            e.printStackTrace();
         }
       }
     }.start();
@@ -248,4 +255,101 @@ public class Robot extends TimedRobot {
     }
 
   }
-}
+
+
+
+
+
+
+
+  // VisionSystemSim visionSim;
+  // RobotMapSim robotMapSim;
+
+  // @Override
+  // public void simulationInit() {
+  //   visionSim = new VisionSystemSim("main");
+  //   robotMapSim = m_robotContainer.m_robotMap.configureSim();
+
+
+  //   // A 0.5 x 0.25 meter rectangular target
+  //   TargetModel targetModel = new TargetModel(0.5, 0.25);
+  //   // The pose of where the target is on the field.
+  //   // Its rotation determines where "forward" or the target x-axis points.
+  //   // Let's say this target is flat against the far wall center, facing the blue driver stations.
+  //   Pose3d targetPose = new Pose3d(16, 4, 2, new Rotation3d(0, 0, Math.PI));
+  //   // The given target model at the given pose
+  //   VisionTargetSim visionTarget = new VisionTargetSim(targetPose, targetModel);
+
+  //   // Add this vision target to the vision system simulation to make it visible
+  //   visionSim.addVisionTargets(visionTarget);
+
+  //   // The layout of AprilTags which we want to add to the vision system
+  //   AprilTagFieldLayout tagLayout = Constants.FieldConstants.kTagLayout;
+
+  //   visionSim.addAprilTags(tagLayout);
+
+
+  //   visionSim.addCamera(robotMapSim.leftCamera, Constants.VisionConstants.LEFT_CAMERA_POS);
+  //   visionSim.addCamera(robotMapSim.rightCamera, Constants.VisionConstants.RIGHT_CAMERA_POS);
+
+  //   SmartDashboard.putData(visionSim.getDebugField());
+  // }
+
+
+  // @Override
+  // public void simulationPeriodic() {
+  //   m_robotContainer.m_robotSwerveDrive.updateSim(RobotController.getBatteryVoltage());
+  //   visionSim.update(m_robotContainer.m_robotSwerveDrive.getPose2d());
+
+  //   // m_robotContainer.m_robotSwerveDrive.
+  // }
+
+
+
+// Start AdvantageKit logging / replay and record metadata
+// Refrence: https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/template_projects/sources/skeleton/src/main/java/frc/robot/Robot.java
+  public void startLogging() {
+    // Record metadata
+    Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
+    Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
+    Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
+    Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
+    Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
+    switch (BuildConstants.DIRTY) {
+      case 0:
+        Logger.recordMetadata("GitDirty", "All changes committed");
+        break;
+      case 1:
+        Logger.recordMetadata("GitDirty", "Uncomitted changes");
+        break;
+      default:
+        Logger.recordMetadata("GitDirty", "Unknown");
+        break;
+    }
+
+    // Set up data receivers & replay source
+    switch (SimConstants.currentMode) {
+      case REAL:
+        // Running on a real robot, log to a USB stick ("/U/logs")
+        Logger.addDataReceiver(new WPILOGWriter());
+        Logger.addDataReceiver(new NT4Publisher());
+        break;
+
+      case SIM:
+        // Running a physics simulator, log to NT
+        Logger.addDataReceiver(new NT4Publisher());
+        break;
+
+      case REPLAY:
+        // Replaying a log, set up replay source
+        setUseTiming(false); // Run as fast as possible
+        String logPath = LogFileUtil.findReplayLog();
+        Logger.setReplaySource(new WPILOGReader(logPath));
+        Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
+        break;
+    }
+
+    // Start AdvantageKit logger
+    Logger.start();
+  }
+}