camera delay calibrator

lib/src/main/java/org/team100/lib/config/Camera.java

@@ -121,6 +121,7 @@ public enum Camera {
 
     UNKNOWN(null, new Transform3d());
 
+    private static final boolean DEBUG = false;
     private static Map<String, Camera> cameras = new HashMap<>();
     static {
         for (Camera i : Camera.values()) {
@@ -138,7 +139,8 @@ private Camera(String serialNumber, Transform3d offset) {
     public static Camera get(String serialNumber) {
         if (cameras.containsKey(serialNumber))
             return cameras.get(serialNumber);
-        System.out.printf("*** Using Camera UNKNOWN for serial number %s\n", serialNumber);
+        if (DEBUG)
+            System.out.printf("*** Using Camera UNKNOWN for serial number %s\n", serialNumber);
         return UNKNOWN;
     }
 

lib/src/main/java/org/team100/lib/config/PIDConstants.java

@@ -32,6 +32,11 @@ public static PIDConstants zero(LoggerFactory log) {
         return new PIDConstants(log, 0, 0);
     }
 
+    /**
+     * PID units for REV motor outboard velocity control are duty cycle per RPM, so
+     * if you want to control to a few hundred RPM, P should be something like
+     * 0.0002.
+     */
     public static PIDConstants makeVelocityPID(LoggerFactory log, double p) {
         return new PIDConstants(log, 0, p);
     }

lib/src/main/java/org/team100/lib/motor/rev/NeoVortexCANSparkMotor.java

@@ -31,7 +31,12 @@ public NeoVortexCANSparkMotor(
                 neutral, motorPhase, statorCurrentLimit, ff, pid);
     }
 
-    /** Real or simulated depending on identity */
+    /**
+     * Real or simulated depending on identity.
+     * 
+     * PID units for outboard velocity control are duty cycle per RPM, so if you
+     * want to control to a few hundred RPM, P should be something like 0.0002.
+     */
     public static BareMotor get(
             LoggerFactory log, CanId can, MotorPhase phase, int statorLimit,
             Feedforward100 ff, PIDConstants pid) {

lib/src/main/java/org/team100/lib/network/CameraReader.java

@@ -16,6 +16,7 @@
 import edu.wpi.first.networktables.PubSubOption;
 import edu.wpi.first.networktables.ValueEventData;
 import edu.wpi.first.util.struct.StructBuffer;
+import edu.wpi.first.wpilibj.Timer;
 
 /**
  * Reads camera input from network tables, which is always a StructArray.
@@ -31,6 +32,7 @@ public abstract class CameraReader<T> {
     private static final int QUEUE_DEPTH = 10;
 
     private final DoubleLogger m_log_timestamp;
+    private final DoubleLogger m_log_age;
     /** e.g. "blips" or "Rotation3d" */
     private final String m_ntValueName;
     /** Manages the queue of incoming messages. */
@@ -44,7 +46,8 @@ public CameraReader(
             String ntValueName,
             StructBuffer<T> buf) {
         LoggerFactory log = parent.type(this);
-        m_log_timestamp = log.doubleLogger(Level.TRACE, "timestamp");
+        m_log_timestamp = log.doubleLogger(Level.TRACE, "timestamp (s)");
+        m_log_age = log.doubleLogger(Level.TRACE, "age (s)");
         m_ntValueName = ntValueName;
         NetworkTableInstance inst = NetworkTableInstance.getDefault();
         m_poller = new NetworkTableListenerPoller(inst);
@@ -113,9 +116,12 @@ public void update() {
 
             // time is in microseconds
             // https://docs.wpilib.org/en/stable/docs/software/networktables/networktables-intro.html#timestamps
-            // NT provides a local time comparable to FPGATime, which is what the history uses.
+            // NT provides a local time comparable to FPGATime, which is what the history
+            // uses.
             double valueTimestamp = ((double) ntValue.getTime()) / 1000000.0;
+            double age = Timer.getFPGATimestamp() - valueTimestamp;
             m_log_timestamp.log(() -> valueTimestamp);
+            m_log_age.log(() -> age);
             if (DEBUG) {
                 System.out.printf("reader timestamp %f\n", valueTimestamp);
             }

lib/src/main/java/org/team100/lib/network/RawTags.java

@@ -0,0 +1,30 @@
+package org.team100.lib.network;
+
+import java.util.function.ObjDoubleConsumer;
+
+import org.team100.lib.localization.Blip24;
+import org.team100.lib.logging.LoggerFactory;
+
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.util.struct.StructBuffer;
+
+/** Listen to raw tag input from the cameras, for testing. */
+public class RawTags extends CameraReader<Blip24> {
+    private final ObjDoubleConsumer<Transform3d> m_sink;
+
+    public RawTags(LoggerFactory parent, ObjDoubleConsumer<Transform3d> sink) {
+        super(parent, "vision", "blips", StructBuffer.create(Blip24.struct));
+        m_sink = sink;
+    }
+
+    @Override
+    protected void perValue(
+            Transform3d cameraOffset,
+            double valueTimestamp,
+            Blip24[] value) {
+        for (Blip24 b : value) {
+            m_sink.accept(b.blipToTransform(), valueTimestamp);
+        }
+    }
+
+}

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

@@ -29,6 +29,10 @@
  */
 public class AS5048RotaryPositionSensor extends DutyCycleRotaryPositionSensor {
 
+    // package private so simulator can see it
+    static final double SENSOR_MIN = 0.003888;
+    static final double SENSOR_MAX = 0.998058;
+
     public AS5048RotaryPositionSensor(
             LoggerFactory parent,
             RoboRioChannel channel,
@@ -37,11 +41,13 @@ public AS5048RotaryPositionSensor(
         super(parent, channel, inputOffset, drive);
     }
 
-    protected double m_sensorMin() {
-        return 0.003888;
+    @Override
+    protected double sensorMin() {
+        return SENSOR_MIN;
     }
 
-    protected double m_sensorMax() {
-        return 0.998058;
+    @Override
+    protected double sensorMax() {
+        return SENSOR_MAX;
     }
 }

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

@@ -46,12 +46,12 @@ public AnalogRotaryPositionSensor(
     }
 
     @Override
-    protected double m_sensorMin() {
+    protected double sensorMin() {
         return 0.0;
     }
 
     @Override
-    protected double m_sensorMax() {
+    protected double sensorMax() {
         return 1.0;
     }
 

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

@@ -51,9 +51,9 @@ protected RoboRioRotaryPositionSensor(
      */
     protected abstract double getRatio();
 
-    protected abstract double m_sensorMin();
+    protected abstract double sensorMin();
 
-    protected abstract double m_sensorMax();
+    protected abstract double sensorMax();
 
     private int wrap() {
         double current = getWrappedPositionRad();
@@ -88,27 +88,28 @@ public double getWrappedPositionRad() {
         return positionRad;
     }
 
-    /** map to full [0,1] */
-    protected double mapSensorRange(double ratio) {
-        // map sensor range
-        if (ratio < m_sensorMin()) {
-            ratio = m_sensorMin();
+    /**
+     * map to full [0,1]
+     */
+    static double mapSensorRange(double ratio, double sensorMin, double sensorMax) {
+        if (ratio < sensorMin) {
+            ratio = sensorMin;
         }
-        if (ratio > m_sensorMax()) {
-            ratio = m_sensorMax();
+        if (ratio > sensorMax) {
+            ratio = sensorMax;
         }
-        return (ratio - m_sensorMin()) / (m_sensorMax() - m_sensorMin());
+        return (ratio - sensorMin) / (sensorMax - sensorMin);
     }
 
     /**
      * This should be nearly cached.
      * 
      * @return radians, [-pi, pi]
      */
-    protected double getRad() {
+    private double getRad() {
         double ratio = getRatio();
 
-        double posTurns = mapSensorRange(ratio);
+        double posTurns = mapSensorRange(ratio, sensorMin(), sensorMax());
         m_log_position_turns.log(() -> posTurns);
 
         double turnsMinusOffset = posTurns - m_positionOffset;
@@ -124,6 +125,8 @@ protected double getRad() {
         }
     }
 
+
+
     /**
      * Always returns zero.
      * 

lib/src/test/java/org/team100/lib/sensor/position/absolute/wpi/AnalogRotaryPositionSensorTest.java

@@ -64,12 +64,12 @@ void testWrapping() {
         try {
             AnalogInputSim sim = new AnalogInputSim(0);
             sim.setInitialized(true);
-            final double range = sensor.m_sensorMax() - sensor.m_sensorMin();
+            final double range = sensor.sensorMax() - sensor.sensorMin();
             for (double unwrappedRad = -6 * Math.PI; unwrappedRad < 6 * Math.PI; unwrappedRad += 0.1) {
                 double wrapped = MathUtil.inputModulus(unwrappedRad, 0, 2 * Math.PI);
                 double piwrapped = MathUtil.angleModulus(unwrappedRad);
                 double sensorTurns = wrapped / (2 * Math.PI);
-                double ratio = sensorTurns * range + sensor.m_sensorMin();
+                double ratio = sensorTurns * range + sensor.sensorMin();
                 sim.setVoltage(ratio * 5);
                 Cache.refresh();
                 double sensorWrapped = sensor.getWrappedPositionRad();

lib/src/test/java/org/team100/lib/sensor/position/absolute/wpi/DutyCycleRotaryPositionSensorTest.java

@@ -69,12 +69,12 @@ void testWrapping() {
             sim.setFrequency(1000);
             sim.setInitialized(true);
 
-            final double range = sensor.m_sensorMax() - sensor.m_sensorMin();
+            final double range = sensor.sensorMax() - sensor.sensorMin();
             for (double unwrappedRad = -6 * Math.PI; unwrappedRad < 6 * Math.PI; unwrappedRad += 0.1) {
                 double wrapped = MathUtil.inputModulus(unwrappedRad, 0, 2 * Math.PI);
                 double piwrapped = MathUtil.angleModulus(unwrappedRad);
                 double sensorTurns = wrapped / (2 * Math.PI);
-                double ratio = sensorTurns * range + sensor.m_sensorMin();
+                double ratio = sensorTurns * range + sensor.sensorMin();
                 sim.setOutput(ratio);
                 Cache.refresh();
                 double sensorWrapped = sensor.getWrappedPositionRad();