Photon sim

build.gradle

@@ -4,6 +4,9 @@ plugins {
     id "com.peterabeles.gversion" version "1.10"
     id "com.diffplug.spotless" version "6.12.0"
 }
+repositories {
+    mavenLocal()
+}
 
 sourceCompatibility = JavaVersion.VERSION_17
 targetCompatibility = JavaVersion.VERSION_17

simgui-ds.json

@@ -90,12 +90,11 @@
     }
   ],
   "robotJoysticks": [
-    {},
     {
-      "guid": "Keyboard1"
+      "guid": "Keyboard0"
     },
     {
-      "guid": "Keyboard0"
+      "guid": "Keyboard1"
     }
   ]
 }

simgui-window.json

@@ -5,13 +5,20 @@
   "MainWindow": {
     "GLOBAL": {
       "fps": "120",
-      "height": "720",
+      "height": "736",
       "maximized": "0",
       "style": "0",
       "userScale": "2",
-      "width": "1280",
-      "xpos": "537",
-      "ypos": "110"
+      "width": "1440",
+      "xpos": "233",
+      "ypos": "830"
+    }
+  },
+  "Table": {
+    "0x542B5671,2": {
+      "Column 0  Width": "169",
+      "Column 1  Width": "156",
+      "RefScale": "13"
     }
   },
   "Window": {
@@ -22,22 +29,22 @@
     },
     "###Joysticks": {
       "Collapsed": "0",
-      "Pos": "250,465",
+      "Pos": "43,465",
       "Size": "796,155"
     },
     "###NetworkTables": {
       "Collapsed": "0",
-      "Pos": "250,277",
-      "Size": "750,185"
+      "Pos": "327,29",
+      "Size": "475,435"
     },
     "###Other Devices": {
       "Collapsed": "0",
-      "Pos": "1025,20",
+      "Pos": "145,13",
       "Size": "250,695"
     },
     "###System Joysticks": {
       "Collapsed": "0",
-      "Pos": "5,350",
+      "Pos": "20,319",
       "Size": "192,218"
     },
     "###Timing": {
@@ -47,7 +54,7 @@
     },
     "Debug##Default": {
       "Collapsed": "0",
-      "Pos": "60,60",
+      "Pos": "43,60",
       "Size": "400,400"
     },
     "Robot State": {

simgui.json

@@ -12,13 +12,20 @@
     "types": {
       "/AdvantageKit/RealOutputs/Arm/mech2d": "Mechanism2d",
       "/FMSInfo": "FMSInfo",
+      "/LiveWindow/PhotonVisionSystem": "Subsystem",
       "/LiveWindow/Ungrouped/DigitalInput[0]": "Digital Input",
       "/LiveWindow/Ungrouped/DigitalInput[1]": "Digital Input",
       "/LiveWindow/Ungrouped/DigitalInput[2]": "Digital Input",
       "/LiveWindow/Ungrouped/DigitalInput[3]": "Digital Input",
       "/LiveWindow/Ungrouped/DigitalInput[4]": "Digital Input",
       "/LiveWindow/Ungrouped/PIDController[10]": "PIDController",
       "/LiveWindow/Ungrouped/PIDController[11]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[12]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[13]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[14]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[15]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[16]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[17]": "PIDController",
       "/LiveWindow/Ungrouped/PIDController[1]": "PIDController",
       "/LiveWindow/Ungrouped/PIDController[2]": "PIDController",
       "/LiveWindow/Ungrouped/PIDController[3]": "PIDController",
@@ -29,9 +36,40 @@
       "/LiveWindow/Ungrouped/PIDController[8]": "PIDController",
       "/LiveWindow/Ungrouped/PIDController[9]": "PIDController",
       "/LiveWindow/Ungrouped/navX-Sensor[4]": "Gyro",
+      "/SmartDashboard/AutoSelector": "String Chooser",
       "/SmartDashboard/Auton Choice": "String Chooser",
       "/SmartDashboard/Field": "Field2d",
-      "/SmartDashboard/Scheduler": "Scheduler"
+      "/SmartDashboard/Scheduler": "Scheduler",
+      "/SmartDashboard/VisionSystemSim-photonvision/Sim Field": "Field2d",
+      "/SmartDashboard/photonvision Sim Field": "Field2d"
+    }
+  },
+  "NetworkTables": {
+    "Persistent Values": {
+      "open": false
+    },
+    "transitory": {
+      "SmartDashboard": {
+        "Auton Choice": {
+          "open": true
+        },
+        "open": true
+      },
+      "northstar": {
+        "calibration": {
+          "open": true
+        },
+        "open": true
+      },
+      "photonvision": {
+        "camera1": {
+          "open": true
+        },
+        "open": true,
+        "photonvision": {
+          "open": true
+        }
+      }
     }
   }
 }

src/main/java/team3176/robot/Robot.java

@@ -4,6 +4,10 @@
 
 package team3176.robot;
 
+import java.util.HashMap;
+import java.util.Map;
+import java.util.function.BiConsumer;
+
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
@@ -17,6 +21,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import team3176.robot.Constants.RobotType;
+import team3176.robot.subsystems.drivetrain.Drivetrain;
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.cscore.UsbCamera;
 
@@ -82,6 +87,7 @@ public void robotInit() {
       case REPLAY:
         String path = LogFileUtil.findReplayLog();
         logger.setReplaySource(new WPILOGReader(path));
+        logger.addDataReceiver(new NT4Publisher());
         logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(path, "_sim")));
         break;
     }
@@ -104,6 +110,32 @@ public void robotInit() {
       fisheyeThread.start();
     }
 
+    Map<String, Integer> commandCounts = new HashMap<>();
+    BiConsumer<Command, Boolean> logCommandFunction =
+        (Command command, Boolean active) -> {
+          String name = command.getName();
+          int count = commandCounts.getOrDefault(name, 0) + (active ? 1 : -1);
+          commandCounts.put(name, count);
+          Logger.getInstance()
+              .recordOutput(
+                  "CommandsUnique/" + name + "_" + Integer.toHexString(command.hashCode()), active);
+          Logger.getInstance().recordOutput("CommandsAll/" + name, count > 0);
+        };
+    CommandScheduler.getInstance()
+        .onCommandInitialize(
+            (Command command) -> {
+              logCommandFunction.accept(command, true);
+            });
+    CommandScheduler.getInstance()
+        .onCommandFinish(
+            (Command command) -> {
+              logCommandFunction.accept(command, false);
+            });
+    CommandScheduler.getInstance()
+        .onCommandInterrupt(
+            (Command command) -> {
+              logCommandFunction.accept(command, false);
+            });
 
   }
 
@@ -131,7 +163,8 @@ public void disabledInit() {
 
   @Override
   public void disabledPeriodic() {
-    //nan
+    robotContainer.checkAutonomousSelection();
+    robotContainer.checkAllaince();
   }
 
   /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */