Skip to content

Commit

Permalink
Tester Utility
Browse files Browse the repository at this point in the history
New Tester utility to run system check at competitions
  • Loading branch information
Mason-Lam committed Jan 4, 2024
1 parent ff1f4d8 commit 5241574
Show file tree
Hide file tree
Showing 2 changed files with 196 additions and 67 deletions.
94 changes: 27 additions & 67 deletions src/main/java/common/core/subsystems/NAR_PIDSubsystem.java
Original file line number Diff line number Diff line change
@@ -1,13 +1,13 @@
package common.core.subsystems;

import java.util.HashSet;
import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier;

import common.core.controllers.ControllerBase;
import common.utility.Log;
import common.utility.narwhaldashboard.NarwhalDashboard;
import common.utility.shuffleboard.NAR_Shuffleboard;
import common.utility.tester.Tester;
import common.utility.tester.Tester.UnitTest;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.CommandBase;
Expand All @@ -23,36 +23,41 @@
*/
public abstract class NAR_PIDSubsystem extends SubsystemBase {

public class Test {
public double setpoint;
public double time;
public boolean passedTest;
private double startTime;
private double endTime;
/**
* UnitTest specifically for PIDSubsystems.
*/
public class SetpointTest extends UnitTest {

private double prevTime;

/**
* Creates a new Test for the PIDSubsystem
* @param setpoint The setpoint for the test goes to
* @param time The time for the subsystem to reach setpoint
* Creates a setpoint test for the system.
* @param testName Name of the test.
* @param setpoint Setpoint for the system to try and reach.
* @param timeOut The time the system has to reach the setpoint.
*/
public Test(double setpoint, double time) {
this.setpoint = setpoint;
this.time = time;
passedTest = false;
startTime = 0;
endTime = 0;
public SetpointTest(String testName, double setpoint, double timeOut) {
super(testName, runOnce(()-> startPID(setpoint)), ()-> atSetpoint(), timeOut);
prevTime = 0;
Tester.getInstance().addTest(getName(), this);
}

/**
* Updates whether or not the test was passed
* Returns a command that runs the test.
*/
private void check() {
passedTest = time >= (endTime - startTime);
@Override
public CommandBase runTest() {
return sequence(
runOnce(()-> prevTime = Timer.getFPGATimestamp()),
super.runTest(),
runOnce(()-> {
Log.info(testName, "Expected Time: " + timeOut);
Log.info(testName, "Actual Time: " + (Timer.getFPGATimestamp() - prevTime));
})
);
}
}

private final HashSet<Test> unitTests = new HashSet<Test>();

protected final ControllerBase m_controller;
protected boolean m_enabled;
private BooleanSupplier debug;
Expand All @@ -74,7 +79,6 @@ public NAR_PIDSubsystem(ControllerBase controller) {
max = Double.POSITIVE_INFINITY;
safetyThresh = 5;
plateauCount = 0;
NarwhalDashboard.getInstance().addUpdate(this.getName(), ()-> hasPassedTest());
}

@Override
Expand Down Expand Up @@ -162,38 +166,6 @@ public void startPID(double setpoint) {
m_controller.setSetpoint(MathUtil.clamp(debug.getAsBoolean() ? this.setpoint.getAsDouble() : setpoint, min, max));
}

/**
* Runs the list of unit tests given
* @param tests Tests to be run on the subsystem
* @return Sequential Command Group containing all unit tests
*/
public CommandBase runTest(Test... tests) {
final CommandBase[] commands = new CommandBase[tests.length];
for (int index = 0; index < tests.length; index ++) {
commands[index] = runTest(tests[index]);
}
return sequence(commands);
}

/**
* Runs the unit test
* @param test Test for the subsystem
* @return Command which will run the test
*/
private CommandBase runTest(Test test) {
unitTests.add(test);
return sequence(
runOnce(()-> test.passedTest = false),
runOnce(()-> test.startTime = Timer.getFPGATimestamp()),
runOnce(()-> startPID(test.setpoint)),
waitUntil(()-> atSetpoint()),
runOnce(()-> test.endTime = Timer.getFPGATimestamp()),
runOnce(()-> test.check()),
runOnce(()-> Log.info(this.getName() + " Test:", "Expected Time: " + test.time + " Actual Time: " + (test.endTime - test.startTime))),
either(print(this.getName() + " TEST PASSED"), print(this.getName() + " TEST FAILED"), ()-> test.passedTest)
);
}

/**
* Enables continuous input.
*
Expand Down Expand Up @@ -252,18 +224,6 @@ public void disable() {
useOutput(0, 0);
}

/**
* Returns whether or not the subsystem has passed the test
* @return True if tests have been passed and false if tests have failed
*/
public boolean hasPassedTest() {
if (unitTests.size() == 0) return false;
for (final Test test : unitTests) {
if (!test.passedTest) return false;
}
return true;
}

/**
* Returns whether the controller is enabled.
*
Expand Down
169 changes: 169 additions & 0 deletions src/main/java/common/utility/tester/Tester.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,169 @@
package common.utility.tester;

import static edu.wpi.first.wpilibj2.command.Commands.*;
import java.util.function.BooleanSupplier;
import java.util.HashMap;
import java.util.ArrayList;

import common.utility.Log;
import common.utility.narwhaldashboard.NarwhalDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;

/**
* Team 3128's Tester utility class used to run system checks at competitions.
*/
public class Tester {

/**System test */
public static class UnitTest {

protected String testName;
protected CommandBase command;
protected BooleanSupplier condition;
protected TestState testState;
protected double timeOut;

/**
* Creates a unit test.
* @param testName Name of the test.
* @param command Command to run for the test.
* @param condition Condition to see if the test passed.
* @param timeOut Time the test has to run before failing.
*/
public UnitTest(String testName, CommandBase command, BooleanSupplier condition, double timeOut) {
this.testName = testName;
this.command = command;
this.condition = condition;
this.timeOut = timeOut;
testState = TestState.FAILED;
}

/**
* Returns a command that runs the test.
*/
public CommandBase runTest() {
return sequence(
runOnce(()-> testState = TestState.RUNNING),
deadline(
waitUntil(condition).withTimeout(timeOut),
command
),
runOnce(()-> {
testState = condition.getAsBoolean() ? TestState.PASSED : TestState.FAILED;
if (testState == TestState.PASSED) Log.info(testName, testName + "TEST PASSED");
else Log.info(testName, "TEST FAILED");
})
);
}
}

/**Collection of tests to be run for a system */
private class Test extends CommandBase {
private final ArrayList<UnitTest> unitTests;
private final String name;
private TestState state;
private int curIndex;

/**
* Creates a test for a specific system.
* @param name Name of the test or system.
*/
private Test(String name) {
unitTests = new ArrayList<UnitTest>();
this.name = name;
state = TestState.FAILED;
curIndex = 0;
NarwhalDashboard.getInstance().addUpdate(name, ()-> state);
NarwhalDashboard.getInstance().addButton(name, (boolean pressed) -> {
if (pressed) this.schedule();
});
}

/**
* Adds a unit test to be run.
* @param test A unit test for the system.
*/
public void addTest(UnitTest test) {
unitTests.add(test);
}

@Override
public void initialize() {
curIndex = 0;
state = TestState.RUNNING;
Log.info(name, "TEST RUNNING");
if (unitTests.size() == 0) state = TestState.FAILED;
else unitTests.get(0).runTest().schedule();
}

@Override
public void execute() {
if (unitTests.size() == 0) return;

final UnitTest test = unitTests.get(curIndex);
switch(test.testState) {
case FAILED:
state = TestState.FAILED;
return;
case PASSED:
curIndex ++;
if (curIndex == unitTests.size()) {
state = TestState.PASSED;
return;
}
unitTests.get(curIndex).runTest().schedule();
default:
}
}

@Override
public void end(boolean interrupted) {
Log.info(name, "TEST " + state);
}

@Override
public boolean isFinished() {
return state != TestState.RUNNING;
}
}

/**Enum representing test states */
public enum TestState {
FAILED,
RUNNING,
PASSED
}

private static Tester instance;

public static synchronized Tester getInstance() {
if (instance == null) {
instance = new Tester();
}
return instance;
}

private Tester() {}

public HashMap<String, Test> systemTests = new HashMap<String, Test>();

/**
* Adds a unit test to be run for a system.
* @param name Name of the test or system.
* @param test Unit test to be added.
*/
public void addTest(String name, UnitTest test) {
if (!systemTests.containsKey(name)) {
systemTests.put(name, new Test(name));
}
systemTests.get(name).addTest(test);
}

/**
* Runs the unit tests for a system.
* @param name Name of the test or system.
*/
public void runTest(String name) {
systemTests.get(name).schedule();
}
}

0 comments on commit 5241574

Please sign in to comment.