make camera tester work better

lib/src/main/java/org/team100/lib/sensor/position/absolute/wpi/DutyCycleRotaryPositionSensor.java

@@ -23,7 +23,8 @@
  * counter.
  * 
  * Note that for the first few seconds after the sensor is constructed on the
- * RoboRIO, the duty cycle input produces garbage.  So the Robot class should sleep awhile.
+ * RoboRIO, the duty cycle input produces garbage. So the Robot class should
+ * sleep awhile.
  * 
  * Relies on Memo and Takt, so you must put Memo.resetAll() and Takt.update() in
  * Robot.robotPeriodic().

lib/src/main/java/org/team100/lib/util/Rotation2dInterpolator.java

@@ -0,0 +1,22 @@
+package org.team100.lib.util;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.interpolation.Interpolator;
+
+/**
+ * For use with TimeInterpolatableBuffer (or something similar).
+ * 
+ * Note that the buffer class short-cuts exact matches.
+ * 
+ * This exposes an inconsistency (a bug IMHO) in the Rotation2d class,
+ * wherein the interpolated value wraps, and the uninterpolated value does not.
+ * 
+ * So you'll want to wrap the buffer output, e.g. by adding kZero (which wraps
+ * the value!)
+ */
+public class Rotation2dInterpolator implements Interpolator<Rotation2d> {
+    @Override
+    public Rotation2d interpolate(Rotation2d startValue, Rotation2d endValue, double t) {
+        return startValue.interpolate(endValue, t);
+    }
+}

lib/src/main/java/org/team100/lib/util/TimeInterpolatableBuffer100.java

@@ -65,6 +65,8 @@ public void put(double timeS, T value) {
                 if (oldestAgeS < m_historyS)
                     break;
                 m_pastSnapshots.remove(oldestTimeS);
+                if (DEBUG)
+                    System.out.printf("removed %f\n", oldestTimeS);
             }
             m_pastSnapshots.put(timeS, value);
         } finally {
@@ -131,6 +133,9 @@ public T get(double timeSeconds) {
         double timeSpan = topBound.getKey() - bottomBound.getKey();
         double timeFraction = timeSinceBottom / timeSpan;
         if (DEBUG) {
+            System.out.printf("timeSeconds %f bottom %f top %f\n",
+                    timeSeconds, bottomBound.getKey(), topBound.getKey());
+
             System.out.printf("interpolate %f\n", timeFraction);
         }
         return m_interpolator.interpolate(
@@ -164,4 +169,11 @@ public int size() {
     public double lastKey() {
         return m_pastSnapshots.lastKey();
     }
+
+    /** Print the entire buffer */
+    public void dump() {
+        for (var x : m_pastSnapshots.entrySet()) {
+            System.out.printf("%s: %s\n", x.getKey(), x.getValue());
+        }
+    }
 }

lib/src/test/java/org/team100/lib/geometry/Rotation2dTest.java

@@ -0,0 +1,29 @@
+package org.team100.lib.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public class Rotation2dTest {
+
+    // this fails
+    // see https://github.com/wpilibsuite/allwpilib/issues/8617
+    // @Test
+    void testInterpolate() {
+        Rotation2d x = new Rotation2d(100);
+        Rotation2d y = x.interpolate(x, 0);
+        assertEquals(x.getRadians(), y.getRadians(), 1e-9);
+    }
+
+    // this fails
+    // see https://github.com/wpilibsuite/allwpilib/issues/8617
+    // @Test
+    void testPlus() {
+        Rotation2d x = new Rotation2d(100);
+        Rotation2d y = x.plus(Rotation2d.kZero);
+        assertEquals(x.getRadians(), y.getRadians(), 1e-9);
+    }
+
+}

studies/camera_delay/.Glass/glass.json

@@ -0,0 +1,84 @@
+{
+  "NetworkTables": {
+    "Persistent Values": {
+      "open": false
+    },
+    "retained": {
+      "log": {
+        "Friction": {
+          "open": true
+        },
+        "NeoVortexCANSparkMotor": {
+          "open": true
+        },
+        "SimpleDynamics": {
+          "open": true
+        },
+        "open": true
+      }
+    },
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Log Level": "String Chooser",
+      "field": "Field2d"
+    }
+  },
+  "NetworkTables Settings": {
+    "mode": "Client (NT4)",
+    "serverTeam": "100"
+  },
+  "Plots": {
+    "Plot <0>": {
+      "plots": [
+        {
+          "axis": [
+            {
+              "autoFit": true
+            }
+          ],
+          "backgroundColor": [
+            0.0,
+            0.0,
+            0.0,
+            0.8500000238418579
+          ],
+          "height": 272,
+          "series": [
+            {
+              "color": [
+                0.2980392277240753,
+                0.44705885648727417,
+                0.6901960968971252,
+                1.0
+              ],
+              "id": "NT:log/AS5048RotaryPositionSensor/frequency"
+            },
+            {
+              "color": [
+                0.2980392277240753,
+                0.44705885648727417,
+                0.6901960968971252,
+                1.0
+              ],
+              "id": "NT:log/AS5048RotaryPositionSensor/duty cycle"
+            }
+          ]
+        },
+        {
+          "axis": [
+            {
+              "autoFit": true
+            }
+          ],
+          "backgroundColor": [
+            0.0,
+            0.0,
+            0.0,
+            0.8500000238418579
+          ],
+          "height": 272
+        }
+      ]
+    }
+  }
+}

studies/camera_delay/.vscode/settings.json

@@ -57,5 +57,9 @@
     "edu.wpi.first.math.**.proto.*",
     "edu.wpi.first.math.**.struct.*",
   ],
-  "java.dependency.enableDependencyCheckup": false
+  "java.dependency.enableDependencyCheckup": false,
+  "files.autoSave": "afterDelay",
+  "editor.minimap.enabled": false,
+  "editor.inlayHints.enabled": "off",
+  "workbench.editor.revealIfOpen": true
 }

studies/camera_delay/simgui-ds.json

@@ -1,9 +1,19 @@
 {
+  "FMS": {
+    "window": {
+      "visible": false
+    }
+  },
   "Joysticks": {
     "window": {
       "visible": false
     }
   },
+  "System Joysticks": {
+    "window": {
+      "visible": false
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [