Add AprilTagVision.java for testing

Print auto time

src/main/java/frc/robot/Robot.java

@@ -14,12 +14,17 @@
 package frc.robot;
 
 import edu.wpi.first.hal.AllianceStationID;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Threads;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.simulation.DriverStationSim;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import java.util.HashMap;
 import java.util.Map;
 import java.util.function.BiConsumer;
+
+import frc.robot.util.VirtualSubsystem;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
@@ -34,9 +39,12 @@
  * project.
  */
 public class Robot extends LoggedRobot {
-  private Command autonomousCommand;
+  private Command autoCommand;
   private RobotContainer robotContainer;
 
+  private double autoStart;
+  private boolean autoMessagePrinted;
+
   /**
    * This function is run when the robot is first started up and should be used for any
    * initialization code.
@@ -130,12 +138,23 @@ public void robotInit() {
   /** This function is called periodically during all modes. */
   @Override
   public void robotPeriodic() {
-    // Runs the Scheduler. This is responsible for polling buttons, adding
-    // newly-scheduled commands, running already-scheduled commands, removing
-    // finished or interrupted commands, and running subsystem periodic() methods.
-    // This must be called from the robot's periodic block in order for anything in
-    // the Command-based framework to work.
+    Threads.setCurrentThreadPriority(true, 99);
+    VirtualSubsystem.periodicAll();
     CommandScheduler.getInstance().run();
+
+    // Print auto duration
+    if (autoCommand != null) {
+      if (!autoCommand.isScheduled() && !autoMessagePrinted) {
+        if (DriverStation.isAutonomousEnabled()) {
+          System.out.printf(
+                  "*** Auto finished in %.2f secs ***%n", Timer.getFPGATimestamp() - autoStart);
+        } else {
+          System.out.printf(
+                  "*** Auto cancelled in %.2f secs ***%n", Timer.getFPGATimestamp() - autoStart);
+        }
+        autoMessagePrinted = true;
+      }
+    }
   }
 
   /** This function is called once when the robot is disabled. */
@@ -149,11 +168,11 @@ public void disabledPeriodic() {}
   /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
-    autonomousCommand = robotContainer.getAutonomousCommand();
-
-    // schedule the autonomous command (example)
-    if (autonomousCommand != null) {
-      autonomousCommand.schedule();
+    autoStart = Timer.getFPGATimestamp();
+    autoMessagePrinted = false;
+    autoCommand = robotContainer.getAutonomousCommand();
+    if (autoCommand != null) {
+      autoCommand.schedule();
     }
   }
 
@@ -168,8 +187,8 @@ public void teleopInit() {
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
     // this line or comment it out.
-    if (autonomousCommand != null) {
-      autonomousCommand.cancel();
+    if (autoCommand != null) {
+      autoCommand.cancel();
     }
   }