diff --git a/build.gradle b/build.gradle index 9eb9d8f..f66badb 100644 --- a/build.gradle +++ b/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() +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java index 20ec9f3..b61efea 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java @@ -51,21 +51,13 @@ 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(); @@ -73,28 +65,8 @@ public void periodic() { // 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. @@ -102,18 +74,20 @@ public void update() { // 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; @@ -121,17 +95,30 @@ public void update() { // 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(); diff --git a/src/main/java/org/mayheminc/util/History.java b/src/main/java/org/mayheminc/util/History.java index b91707f..b4d75da 100644 --- a/src/main/java/org/mayheminc/util/History.java +++ b/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]; } -} +} \ No newline at end of file diff --git a/src/test/java/org/mayheminc/util/HistoryTest.java b/src/test/java/org/mayheminc/util/HistoryTest.java new file mode 100644 index 0000000..7a9d26b --- /dev/null +++ b/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()); + } +} \ No newline at end of file