Update NT data; history interpolation

build.gradle

@@ -41,7 +41,6 @@ deploy {
 def includeDesktopSupport = false
 
 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
-// Also defines JUnit 4.
 dependencies {
     implementation wpi.deps.wpilib()
     nativeZip wpi.deps.wpilibJni(wpi.platforms.roborio)
@@ -52,7 +51,10 @@ dependencies {
     nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio)
     nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop)
 
-    testImplementation 'junit:junit:4.12'
+    testImplementation 'org.junit.jupiter:junit-jupiter-api:5.6.0'
+    testRuntime 'org.junit.jupiter:junit-jupiter-engine:5.6.0'
+    testCompile 'org.mockito:mockito-core:3.3.0'
+    testCompile 'org.mockito:mockito-junit-jupiter:3.3.0'
 
     // Enable simulation gui support. Must check the box in vscode to enable support
     // upon debugging
@@ -66,3 +68,7 @@ jar {
     from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
     manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
 }
+
+test {
+    useJUnitPlatform()
+}

src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java

@@ -51,87 +51,74 @@ public class Targeting extends SubsystemBase {
   private double[] m_target_array;
   private int m_priorFrameCount;
   private double m_priorFrameTime;
-  private double[] ARRAY_OF_NEG_ONE = { -1.0 };
+  private final double[] ARRAY_OF_NEG_ONE = { -1.0 };
 
   private double m_bestY = 0.0;
   private double m_bestX = 0.0;
   private double m_tilt = 0.0;
   private double m_area = 0.0;
 
-  public enum TargetPosition {
-    LEFT_MOST, CENTER_MOST, RIGHT_MOST, CENTER_OF_RIGHT_CARGO_SHIP, CENTER_OF_LEFT_CARGO_SHIP
-  };
-
-  public enum TargetHeight {
-    CARGO, HATCH
-  };
-
   @Override
   public void periodic() {
     update();
   }
 
   // TODO: make an updateSmartDashboard() method in Targeting for optimization
   public void update() {
-    // perform periodic update functions for the targeting capability
-    int latestFrameCount = (int) SmartDashboard.getNumber("frameCount", -1.0 /* default to -1 */);
-    if (latestFrameCount < 0) {
-      // an invalid number for latestFrameCount, display warning light
-      SmartDashboard.putBoolean("visionOK", false);
-      SmartDashboard.putString("visionOkDebug", "No Data");
-    } else if (latestFrameCount == m_priorFrameCount) {
-      // have not received a new frame. If more than 1 second has elapsed since
-      // prior new frame, display a warning light on the SmartDashboard
-      if (Timer.getFPGATimestamp() > m_priorFrameTime + 1.0) {
-        SmartDashboard.putBoolean("visionOK", false);
-        SmartDashboard.putString("visionOkDebug", "Stale Data");
-      } else {
-        // else, have an old frame, but it's not too old yet, so do nothing
-      }
-    } else {
-      // have received a new frame, save the time and update m_priorFrameCount
-      m_priorFrameTime = Timer.getFPGATimestamp();
-      m_priorFrameCount = latestFrameCount;
-      SmartDashboard.putBoolean("visionOK", true);
-      SmartDashboard.putString("visionOkDebug", "Good Data");
-    }
+    // get the latest output from the targeting camera
+    m_target_array = SmartDashboard.getNumberArray("target", ARRAY_OF_NEG_ONE);
 
     // Update all of the targeting information, as follows:
     // 1 - Determine if we have any valid data in the array.
     // If not, set the "error" to zero, so that the robot thinks
     // it is on target.
     // 2 - Use the target to compute the needed information
 
-    // get the latest output from the targeting camera
-    m_target_array = SmartDashboard.getNumberArray("target", ARRAY_OF_NEG_ONE);
-
     if (m_target_array == null || m_target_array.length == 0) {
-      // this means the key is found, but is empty
+      // This means that the key was set, but to an empty array
+      SmartDashboard.putBoolean("visionOK", false);
+      SmartDashboard.putString("visionOkDebug", "Bad Data");
       m_bestX = 0.0;
       m_bestY = 0.0;
       m_tilt = 0.0;
       m_area = 0.0;
       // m_desiredAzimuth = RobotContainer.turret.getAzimuthForCapturedImage();
     } else if (m_target_array[0] < 0.0) {
-      // this means the array has no valid data. Set m_xError = 0.0
+      // This means the array was not retrieved (and the default value was returned)
+      // Display warning light
+      SmartDashboard.putBoolean("visionOK", false);
+      SmartDashboard.putString("visionOkDebug", "No Data");
       m_bestX = 0.0;
       m_bestY = 0.0;
       m_tilt = 0.0;
       m_area = 0.0;
       // m_desiredAzimuth = RobotContainer.turret.getAzimuthForCapturedImage();
     } else {
       // We have a valid data array.
-      // There are three different situations:
-      // a - We want the left-most target
-      // b - We want the "centered" target
-      // c - We want the right-most target
-
-      // Handle each of them separately;
-      // we need the results in "bestXError" and "bestY"
-      m_bestX = m_target_array[0]; // get the x-value
-      m_bestY = m_target_array[1]; // get the y-value
-      m_tilt = m_target_array[2];
-      m_area = m_target_array[3];
+      final int latestFrameCount = (int) m_target_array[0];
+
+      // perform periodic update functions for the targeting capability
+      if (latestFrameCount == m_priorFrameCount) {
+        // have not received a new frame. If more than 1 second has elapsed since
+        // prior new frame, display a warning light on the SmartDashboard
+        if (Timer.getFPGATimestamp() > m_priorFrameTime + 1.0) {
+          SmartDashboard.putBoolean("visionOK", false);
+          SmartDashboard.putString("visionOkDebug", "Stale Data");
+        } else {
+          // else, have an old frame, but it's not too old yet, so do nothing
+        }
+      } else {
+        // have received a new frame, save the time and update m_priorFrameCount
+        m_priorFrameTime = Timer.getFPGATimestamp();
+        m_priorFrameCount = latestFrameCount;
+        SmartDashboard.putBoolean("visionOK", true);
+        SmartDashboard.putString("visionOkDebug", "Good Data");
+      }
+
+      m_bestX = m_target_array[1]; // get the x-value
+      m_bestY = m_target_array[2]; // get the y-value
+      m_tilt = m_target_array[3];
+      m_area = m_target_array[4];
 
       m_desiredAzimuth = findDesiredAzimuth(m_bestX, m_bestY, m_tilt, m_area);
       m_desiredHood = getHoodFromY();

src/main/java/org/mayheminc/util/History.java

@@ -15,45 +15,41 @@ public History() {
     }
 
     public void add(double t, double az) {
-
         time[index] = t;
         azimuth[index] = az;
-
         index++;
-        if (index >= HISTORY_SIZE) {
-            index = 0;
-        }
+        index %= HISTORY_SIZE;
+    }
+
+    protected void reportError(String message) {
+        DriverStation.reportError("Looking too far back", false);
     }
 
     public double getAzForTime(double t) {
-        double az = azimuth[index];
-        int i = index - 1;
-        int count = 0;
-
-        // if (t < 0) {
-        // DriverStation.reportError("Negative time in history", false);
-        // return 0.0; // no negative times.
-        // }
-
-        while (i != index) {
-            if (i < 0) {
-                i = HISTORY_SIZE - 1;
-            }
+        int i = index; // Start just after last entry
 
+        do {
+            // Move to one entry earlier
+            i = (i + HISTORY_SIZE - 1) % HISTORY_SIZE;
+
+            // Check if the time entry is old enough
             if (time[i] <= t) {
-                az = azimuth[i];
-                break;
-            }
-
-            i--;
-            count++;
-            if (count > HISTORY_SIZE) {
-                DriverStation.reportError("Looking too far back", false);
-                az = azimuth[index];
-                break;
+                int prev = (i + 1) % HISTORY_SIZE;
+                if (prev != index && time[i] >= 0.0) {
+                    // Interpolate between the two closest entries
+                    assert(time[i] < time[prev]);
+                    double factor = (t - time[i]) / (time[prev] - time[i]);
+                    return azimuth[i] + factor * (azimuth[prev] - azimuth[i]);
+                } else {
+                    // Only one (real) entry; no interpolation possible
+                    return azimuth[i];
+                }
             }
-        }
+        } while (i != index);
 
-        return az;
+        // Oldest entry is still newer than the requested timestamp
+        // Return the oldest entry in the history 
+        reportError("Looking too far back");
+        return azimuth[index];
     }
-}
+}

src/test/java/org/mayheminc/util/HistoryTest.java

@@ -0,0 +1,64 @@
+package org.mayheminc.util;
+
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.extension.ExtendWith;
+import static org.junit.jupiter.api.Assertions.*;
+
+import org.mockito.junit.jupiter.MockitoExtension;
+import static org.mockito.Mockito.*;
+
+@ExtendWith(MockitoExtension.class)
+public class HistoryTest {
+    @Test
+    public void runHistoryTest() {
+        History hist = spy(History.class);
+
+        // Stub out the error reporting
+        doNothing().when(hist).reportError(anyString());
+
+        // Returns 0 by default before any data
+        assertEquals(0.0, hist.getAzForTime(1.1));
+        assertEquals(0.0, hist.getAzForTime(0.1));
+        verify(hist, never()).reportError(anyString());
+
+        // Add first "real" data
+        hist.add(1.0, 101.0);
+        assertEquals(101.0, hist.getAzForTime(1.0));
+        assertEquals(101.0, hist.getAzForTime(2.0));
+        assertEquals(0.0, hist.getAzForTime(0.1));
+        verify(hist, never()).reportError(anyString());
+
+        // Add second entry and check interpolation
+        hist.add(2.0, 102.0);
+        assertEquals(102.0, hist.getAzForTime(2.0));
+        assertEquals(101.0, hist.getAzForTime(1.0));
+        assertEquals(101.5, hist.getAzForTime(1.5));
+        assertEquals(101.8, hist.getAzForTime(1.8));
+        verify(hist, never()).reportError(anyString());
+
+        // Fill history -- depends on the value of HISTORY_SIZE
+        for (int i=3; i <= 50; i++) {
+            hist.add(i, 100 + i);
+        }
+        assertEquals(150.0, hist.getAzForTime(50));
+        assertEquals(150.0, hist.getAzForTime(51));
+        assertEquals(150.0, hist.getAzForTime(100));
+        assertEquals(101.0, hist.getAzForTime(0.1));
+        verify(hist, times(1)).reportError(anyString());
+        assertEquals(101.0, hist.getAzForTime(1.0));
+        assertEquals(102.0, hist.getAzForTime(2.0));
+        verify(hist, times(1)).reportError(anyString());
+
+        // Overflow history
+        hist.add(51, 151);
+        assertEquals(151.0, hist.getAzForTime(51));
+        assertEquals(151.0, hist.getAzForTime(100));
+        assertEquals(102.0, hist.getAzForTime(0.1));
+        verify(hist, times(2)).reportError(anyString());
+        assertEquals(102.0, hist.getAzForTime(1.0));
+        verify(hist, times(3)).reportError(anyString());
+        assertEquals(102.0, hist.getAzForTime(2.0));
+        assertEquals(150.7, hist.getAzForTime(50.7));
+        verify(hist, times(3)).reportError(anyString());
+    }
+}