diff --git a/.idea/kotlinc.xml b/.idea/kotlinc.xml index 8b7f4afd..3b930d30 100644 --- a/.idea/kotlinc.xml +++ b/.idea/kotlinc.xml @@ -3,4 +3,9 @@ + + + \ No newline at end of file diff --git a/README.md b/README.md index 039138b0..72c78984 100644 --- a/README.md +++ b/README.md @@ -48,7 +48,9 @@ A system to easily set tuning fields through WPILib `Preferences`. Various classes for testing common things. - `BurnoutTester` for testing if motors are burned out. +- `EncoderTester` for testing if motors have working encoders attached. - `SimpleControllersTester` for easily running motors. +- `ControllersMultiTester` for automatically running multiple tests across motor groups. #### Triggers diff --git a/build.gradle b/build.gradle index b4c9a5f1..90414b38 100644 --- a/build.gradle +++ b/build.gradle @@ -27,6 +27,7 @@ subprojects { repositories { mavenCentral() + maven {url "https://jitpack.io/"} } dependencies { diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 933b6473..1510c29a 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,6 @@ +#Fri Nov 30 17:46:10 PST 2018 distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-4.4-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-4.4-all.zip diff --git a/lib/build.gradle b/lib/build.gradle index 8b81dcb5..fbee41ea 100644 --- a/lib/build.gradle +++ b/lib/build.gradle @@ -45,6 +45,7 @@ dependencies { compile "jaci.pathfinder:Pathfinder-Java:1.8" compile "openrio.powerup:MatchData:2018.01.07" compile "com.google.guava:guava:27.0-jre" - // https://mvnrepository.com/artifact/org.apache.commons/commons-math3 - compile 'org.apache.commons:commons-math3:3.6.1' + compile "org.apache.commons:commons-math3:3.6.1" + // Last release was in 2014, so we're just pinning it to this commit instead + compile "com.github.oxo42:stateless4j:3dd512049f" } diff --git a/lib/src/main/java/org/team1540/rooster/testers/AbstractTester.java b/lib/src/main/java/org/team1540/rooster/testers/AbstractTester.java index b6562d35..56eabb27 100644 --- a/lib/src/main/java/org/team1540/rooster/testers/AbstractTester.java +++ b/lib/src/main/java/org/team1540/rooster/testers/AbstractTester.java @@ -22,10 +22,10 @@ public abstract class AbstractTester implements Tester { private int updateDelay; private boolean running = true; @NotNull - List itemsToTest; + private List itemsToTest; @NotNull private Function test; - @NotNull + @Nullable private List> runConditions; @NotNull private Map> storedResults; @@ -38,8 +38,8 @@ public abstract class AbstractTester implements Tester { * @param runConditions The conditions that must be met before the test will be executed on an * item. */ - AbstractTester(@NotNull Function test, @NotNull List itemsToTest, - @NotNull List> runConditions) { + protected AbstractTester(@NotNull Function test, @NotNull List itemsToTest, + @Nullable List> runConditions) { this(test, itemsToTest, runConditions, (int) DEFAULT_LOG_TIME / (DEFAULT_UPDATE_DELAY / 1000)); } @@ -55,8 +55,8 @@ public abstract class AbstractTester implements Tester { * checked against while running. * @param updateDelay The delay between the test being run on the items. */ - AbstractTester(@NotNull Function test, @NotNull List itemsToTest, - @NotNull List> runConditions, float logTime, int updateDelay) { + protected AbstractTester(@NotNull Function test, @NotNull List itemsToTest, + @Nullable List> runConditions, float logTime, int updateDelay) { this(test, itemsToTest, runConditions, (int) (logTime / ((float) updateDelay / 1000f))); this.updateDelay = updateDelay; @@ -71,8 +71,9 @@ public abstract class AbstractTester implements Tester { * item. * @param queueDepth The maximum number of items that the {@link EvictingQueue} can hold. */ - AbstractTester(@NotNull Function test, @NotNull List itemsToTest, - @NotNull List> runConditions, int queueDepth) { + @SuppressWarnings("WeakerAccess") + protected AbstractTester(@NotNull Function test, @NotNull List itemsToTest, + @Nullable List> runConditions, int queueDepth) { this.test = test; this.itemsToTest = itemsToTest; this.runConditions = runConditions; @@ -95,11 +96,18 @@ public void setTest(@NotNull Function test) { } @Override - @NotNull + @Nullable public List> getRunConditions() { - return Collections.unmodifiableList(runConditions); + return runConditions; + } + + @Override + public void setRunConditions( + @Nullable List> runConditions) { + this.runConditions = runConditions; } + @Override @NotNull public List getItemsToTest() { @@ -141,14 +149,18 @@ public boolean setRunning(boolean status) { * The code that should be called every tick. This does the actual testing. Override me as * necessary (but don't forget to call super!) */ - void periodic() { + protected void periodic() { for (T t : itemsToTest) { - // Run through all the run conditions and make sure they all return true - for (Function runCondition : runConditions) { - if (!runCondition.apply(t)) { - return; + // If there are run conditions + if (runConditions != null) { + // Run through all the run conditions and make sure they all return true + for (Function runCondition : runConditions) { + if (!runCondition.apply(t)) { + return; + } } } + this.storedResults.get(t).addResult(this.test.apply(t), System.currentTimeMillis()); } } diff --git a/lib/src/main/java/org/team1540/rooster/testers/BurnoutTester.java b/lib/src/main/java/org/team1540/rooster/testers/BurnoutTester.java deleted file mode 100644 index a21fb60e..00000000 --- a/lib/src/main/java/org/team1540/rooster/testers/BurnoutTester.java +++ /dev/null @@ -1,115 +0,0 @@ -package org.team1540.rooster.testers; - -import edu.wpi.first.wpilibj.Sendable; -import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; -import java.util.Arrays; -import java.util.Collections; -import java.util.List; -import java.util.Optional; -import org.apache.commons.math3.stat.descriptive.moment.StandardDeviation; -import org.apache.commons.math3.stat.descriptive.rank.Median; -import org.team1540.rooster.wrappers.ChickenTalon; - -/** - * Reports motor burnouts by comparing the current draw across a series of similarly-purposed - * motors and reporting low outliers. - */ -@SuppressWarnings("unused") -public class BurnoutTester extends AbstractTester implements Sendable { - - - private static final Median medianCalculator = new Median(); - private static final StandardDeviation stdDevCalculator = new StandardDeviation(); - private double medianCurrent = 0; - private double stdDevCurrent = 0; - - private String name = "BurnoutTester"; - - /** - * Construct a new instance. - * - * @param motorsToTest The motors to compare to each other. - */ - public BurnoutTester(ChickenTalon... motorsToTest) { - // Because passing in a reference to a non-static method in the constructor doesn't work. - super((stupid) -> null, Arrays.asList(motorsToTest), - Collections.singletonList((ignore) -> true), 150, 500); - this.setTest(this::testBurnout); - this.setUpdateDelay(500); - } - - /** - * Construct a new instance. - * - * @param motorsToTest The motors to compare to each other. - */ - public BurnoutTester(List motorsToTest) { - // Because passing in a reference to a non-static method in the constructor doesn't work. - super((stupid) -> null, motorsToTest, - Collections.singletonList((ignore) -> true), 150, 500); - this.setTest(this::testBurnout); - } - - /** - * Tests to see if a motor is burned out by checking to see if it is at least one standard - * deviation below the median. - * @param manageable The motor to test for burnout. - * @return Boolean indicating burnout. - */ - @SuppressWarnings("WeakerAccess") - public Boolean testBurnout(ChickenTalon manageable) { - return manageable.getOutputCurrent() < (this.medianCurrent - 1 * this.stdDevCurrent); - } - - /** - * Gets the currents, calculates the median and standard deviation, then calls super. - */ - @Override - void periodic() { - double[] currents = itemsToTest.stream().mapToDouble(ChickenTalon::getOutputCurrent).toArray(); - medianCurrent = medianCalculator.evaluate(currents); - stdDevCurrent = stdDevCalculator.evaluate(currents); - super.periodic(); - } - - @Override - public String getName() { - return this.name; - } - - @Override - public void setName(String name) { - this.name = name; - } - - @Override - public String getSubsystem() { - return this.name; - } - - @Override - public void setSubsystem(String subsystem) { - this.name = subsystem; - } - - /** - * Displays the current status of each motor and the median current draw. - * @param builder The {@link SendableBuilder} to use. - */ - @Override - public void initSendable(SendableBuilder builder) { - for (ChickenTalon t : getItemsToTest()) { - // Get the most recent value if present, else simply don't add it to the builder - builder.addBooleanProperty(t.getDeviceID() + "", () -> { - // TODO probably cleaner version of this, at the least ifPresentOrElse() in Java 9 - Optional> result = Optional.ofNullable(peekMostRecentResult(t)); - if (result.isPresent()) { - return result.get().getResult(); - } else { - return false; - } - }, null); - } - builder.addDoubleProperty("Median current", () -> medianCurrent, null); - } -} diff --git a/lib/src/main/java/org/team1540/rooster/testers/Tester.java b/lib/src/main/java/org/team1540/rooster/testers/Tester.java index f2efa4da..51e98dae 100644 --- a/lib/src/main/java/org/team1540/rooster/testers/Tester.java +++ b/lib/src/main/java/org/team1540/rooster/testers/Tester.java @@ -23,12 +23,21 @@ public interface Tester extends Runnable { /** * Gets the conditions that must be met before the test will be executed on an item. * - * @return An {@link EvictingQueue} of the run conditions. + * @return An potentially {@link List} of the run conditions, or null if there are none (always + * execute.) */ @SuppressWarnings("UnstableApiUsage") - @NotNull // TODO how to annotate as unmodifiable? + @Nullable List> getRunConditions(); + /** + * Sets the run conditions that must be met before the test will be executed on an item. + * + * @param runConditions A {@link List} of the run conditions, or null if the test should always + * run. + */ + void setRunConditions(@Nullable List> runConditions); + /** * Gets the items that the tests are being applied to. * diff --git a/lib/src/main/java/org/team1540/rooster/testers/motor/BurnoutTester.java b/lib/src/main/java/org/team1540/rooster/testers/motor/BurnoutTester.java new file mode 100644 index 00000000..9ae39f9d --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/testers/motor/BurnoutTester.java @@ -0,0 +1,174 @@ +package org.team1540.rooster.testers.motor; + +import com.ctre.phoenix.motorcontrol.IMotorController; +import edu.wpi.first.wpilibj.Sendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; +import java.util.Arrays; +import java.util.List; +import java.util.Optional; +import org.apache.commons.math3.stat.descriptive.moment.StandardDeviation; +import org.apache.commons.math3.stat.descriptive.rank.Median; +import org.team1540.rooster.testers.AbstractTester; +import org.team1540.rooster.testers.ResultWithMetadata; + +/** + * Reports motor burnouts by comparing the current draw across a series of similarly-purposed + * motors and reporting low outliers or checking if a single motor is below the cutoff. + */ +@SuppressWarnings("unused") +public class BurnoutTester extends AbstractTester implements Sendable { + + private static final Median medianCalculator = new Median(); + private static final StandardDeviation stdDevCalculator = new StandardDeviation(); + private double medianCurrent = 0; + private double stdDevCurrent = 0; + + private double currentCutoff = 1; + private double percentOutputCutoff = 0.5; + + private String name = "BurnoutTester"; + + /** + * Construct a new instance with the default logTime of 150 seconds and an update delay of 500 + * ms, using the {@link BurnoutTester#testBurnoutMultiMotor(IMotorController)} if there is more + * than one motor and {@link BurnoutTester#testBurnoutSingleMotor(IMotorController)} if there is + * one or few motors. Equivalent to {@link BurnoutTester#BurnoutTester(List) EncoderTester + * (Arrays.asList(motorsToTest))}. + * + * @param motorsToTest The motors to compare to each other. + */ + @SuppressWarnings("WeakerAccess") + public BurnoutTester(IMotorController... motorsToTest) { + this(Arrays.asList(motorsToTest)); + } + + /** + * Construct a new instance with the default logTime of 150 seconds and an update delay of 500 + * ms, using the {@link BurnoutTester#testBurnoutMultiMotor(IMotorController)} if there are more + * than two motor sand {@link BurnoutTester#testBurnoutSingleMotor(IMotorController)} if there two + * two or few motors. + * + * @param motorsToTest The motors to compare to each other. + */ + @SuppressWarnings("WeakerAccess") + public BurnoutTester(List motorsToTest) { + // Because passing in a reference to a non-static method in the constructor doesn't work. + super((stupid) -> null, motorsToTest, null, 150, 500); + this.setTest(motorsToTest.size() > 2 ? this::testBurnoutMultiMotor : + this::testBurnoutSingleMotor); + } + + /** + * Tests to see if a motor is burned out by checking to see if the current being drawn is at + * least one standard deviation below the median. + * @param controller The motor to test for burnout. + * @return Boolean indicating burnout. + */ + @SuppressWarnings("WeakerAccess") + public boolean testBurnoutMultiMotor(IMotorController controller) { + return controller.getOutputCurrent() < (this.medianCurrent - 1 * this.stdDevCurrent); + } + + /** + * Test to see if a motor is burned out by checking to see if the current being drawn is below + * the current cutoff. + * + * @param controller The motor to test for burnout. + * @return Boolean indicating burnout. + */ + @SuppressWarnings("WeakerAccess") + public boolean testBurnoutSingleMotor(IMotorController controller) { + return controller.getMotorOutputPercent() > percentOutputCutoff + && controller.getOutputCurrent() < currentCutoff; + } + + /** + * Gets the currents, calculates the median and standard deviation, then calls super. + */ + @Override + protected void periodic() { + double[] currents = getItemsToTest().stream().mapToDouble(IMotorController::getOutputCurrent) + .toArray(); + medianCurrent = medianCalculator.evaluate(currents); + stdDevCurrent = stdDevCalculator.evaluate(currents); + super.periodic(); + } + + @Override + public String getName() { + return this.name; + } + + @Override + public void setName(String name) { + this.name = name; + } + + @Override + public String getSubsystem() { + return this.name; + } + + @Override + public void setSubsystem(String subsystem) { + this.name = subsystem; + } + + /** + * Gets the current cutoff. Defaults to 1A. + * + * @return The current cutoff in amps. + */ + public double getCurrentCutoff() { + return currentCutoff; + } + + /** + * Sets the current cutoff. + * + * @param currentCutoff The current cutoff in amps. + */ + public void setCurrentCutoff(double currentCutoff) { + this.currentCutoff = currentCutoff; + } + + /** + * Gets the percent output cutoff. Defaults to 50%. + * + * @return The percent output cutoff as a percentage. + */ + public double getPercentOutputCutoff() { + return percentOutputCutoff; + } + + /** + * Sets the percent output cutoff. + * + * @param percentOutputCutoff The output cutoff as a percentage. + */ + public void setPercentOutputCutoff(double percentOutputCutoff) { + this.percentOutputCutoff = percentOutputCutoff; + } + + /** + * Displays the current status of each motor and the median current draw. + * @param builder The {@link SendableBuilder} to use. + */ + @Override + public void initSendable(SendableBuilder builder) { + //noinspection Duplicates + for (IMotorController t : getItemsToTest()) { + // Get the most recent value if present, else simply don't add it to the builder + builder.addBooleanProperty(t.getDeviceID() + "", () -> { + // TODO probably cleaner version of this, at the least ifPresentOrElse() in Java 9 + Optional> result = Optional.ofNullable(peekMostRecentResult(t)); + if (result.isPresent()) { + return result.get().getResult(); + } else { + return false; + } + }, null); + } + builder.addDoubleProperty("Median current", () -> medianCurrent, null); + } +} diff --git a/lib/src/main/java/org/team1540/rooster/testers/motor/ControllersMultiTester.java b/lib/src/main/java/org/team1540/rooster/testers/motor/ControllersMultiTester.java new file mode 100644 index 00000000..14f12d0e --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/testers/motor/ControllersMultiTester.java @@ -0,0 +1,207 @@ +package org.team1540.rooster.testers.motor; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.IMotorController; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.github.oxo42.stateless4j.StateMachine; +import com.github.oxo42.stateless4j.StateMachineConfig; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.command.Command; +import java.util.LinkedList; +import java.util.List; +import java.util.function.Consumer; +import org.team1540.rooster.testers.AbstractTester; +import org.team1540.rooster.testers.ResultWithMetadata; +import org.team1540.rooster.wrappers.ChickenTalon; + +/** + * Simple automated testing for motors. Add the motors with the specified test using the builder + * methods, then run the command. + */ +public class ControllersMultiTester extends Command { + + private Timer timer = new Timer(); + private StateMachine stateMachine; + private List tests = new LinkedList<>(); + private int index = 0; + + /** + * Default constructor. Note that this class follows a builder pattern; use + * {@link #addControllerGroup(IMotorController...)} and + * {@link #addEncoderGroup(ChickenTalon...)} to add the motors. + */ + public ControllersMultiTester() { + StateMachineConfig stateMachineConfig = new StateMachineConfig<>(); + stateMachineConfig.configure(State.INITIALIZING) + .permit(Trigger.TIME_HAS_PASSED, State.SPIN_UP); + stateMachineConfig.configure(State.SPIN_UP) + .permit(Trigger.TIME_HAS_PASSED, State.EXECUTING) + .onEntry(() -> { + for (IMotorController motor : tests.get(index).getTest().getItemsToTest()) { + tests.get(index).getFunction().accept(motor); + } + }); + stateMachineConfig.configure(State.EXECUTING) + .permit(Trigger.TIME_HAS_PASSED, State.SPIN_DOWN) + .onEntry(() -> new Thread(tests.get(index).getTest()).start()) + .onExit(() -> tests.get(index).getTest().setRunning(false)); + stateMachineConfig.configure(State.SPIN_DOWN) + .permit(Trigger.TIME_HAS_PASSED, State.SPIN_UP) + .permit(Trigger.FINISHED, State.FINISHED) + .onEntry(() -> { + for (IMotorController motor : tests.get(index).getTest().getItemsToTest()) { + motor.set(ControlMode.PercentOutput, 0); + } + index++; + }); + this.stateMachine = new StateMachine<>(State.INITIALIZING, stateMachineConfig); + this.stateMachine.setShouldLog(false); + } + + /** + * Add a group of motors to test together with the default function (disables braking and sets + * the motors to full.) Currently uses only {@link BurnoutTester}. + * + * @param controllerGroup The motors to add. + * @return this + */ + public ControllersMultiTester addControllerGroup(IMotorController... controllerGroup) { + addControllerGroup(this::setMotorToFull, controllerGroup); + return this; + } + + /** + * Add a group of motors to test together with the specified function. Currently uses only + * {@link BurnoutTester}. + * @param function The function to apply before running the tests. + * @param controllerGroup The motors to add. + * @return this + */ + @SuppressWarnings({"UnusedReturnValue", "WeakerAccess"}) + public ControllersMultiTester addControllerGroup(Consumer function, + IMotorController... controllerGroup) { + tests.add(new TesterAndCommand(new BurnoutTester(controllerGroup), function)); + return this; + } + + /** + * Add a group of motors with encoders to test together with the default function (disables + * braking and sets the motors to full.) Currently uses only {@link EncoderTester}. + * @param controllerGroup The motors to add. + * @return this + */ + public ControllersMultiTester addEncoderGroup(ChickenTalon... controllerGroup) { + addEncoderGroup(this::setMotorToFull, controllerGroup); + return this; + } + + /** + * Add a group of motors with encoders to test together with the specified function. Currently + * uses only {@link EncoderTester}. + * @param function The function to apply before running the tests. + * @param controllerGroup The motors to add. + * @return this + */ + @SuppressWarnings({"UnusedReturnValue", "WeakerAccess"}) + public ControllersMultiTester addEncoderGroup(Consumer function, + ChickenTalon... controllerGroup) { + tests.add(new TesterAndCommand(new EncoderTester(controllerGroup), function)); + return this; + } + + @Override + protected void initialize() { + timer.reset(); + timer.start(); + } + + @Override + protected void execute() { + if (stateMachine.getState().timeToComplete != null) { + if (timer.hasPeriodPassed(stateMachine.getState().getTimeToComplete())) { + if (index >= tests.size()) { + stateMachine.fire(Trigger.FINISHED); + } else { + stateMachine.fire(Trigger.TIME_HAS_PASSED); + } + } + } + } + + @Override + protected void end() { + // Very optimized yes no duplicate code either + for (TesterAndCommand testerAndCommand : tests) { + for (IMotorController controller : testerAndCommand.getTest().getItemsToTest()) { + int failureCount = 0; + for (ResultWithMetadata result : testerAndCommand.getTest() + .getStoredResults(controller)) { + if (result.getResult().equals(Boolean.TRUE)) { + failureCount++; + } + } + if (failureCount > 0) { + DriverStation + .reportError("Motor " + controller.getDeviceID() + " reported " + failureCount + + " failures of type " + testerAndCommand.getTest(), false); + } + } + } + System.out.println("Finished testing"); + } + + @Override + protected boolean isFinished() { + return stateMachine.getState().equals(State.FINISHED); + } + + private void setMotorToFull(IMotorController motor) { + motor.setNeutralMode(NeutralMode.Coast); + motor.set(ControlMode.PercentOutput, 1.0); + } + + private enum State { + INITIALIZING(0), SPIN_UP(0.25), EXECUTING(1), SPIN_DOWN(0), FINISHED; + + private final Double timeToComplete; + + State() { + this.timeToComplete = null; + } + + State(double timeToComplete) { + this.timeToComplete = timeToComplete; + } + + public Double getTimeToComplete() { + return timeToComplete; + } + } + + private enum Trigger { + TIME_HAS_PASSED, FINISHED + } + + private class TesterAndCommand { + + private AbstractTester test; + private Consumer function; + + private TesterAndCommand( + AbstractTester test, + Consumer function) { + this.test = test; + this.function = function; + } + + private AbstractTester getTest() { + return test; + } + + private Consumer getFunction() { + return function; + } + } + +} diff --git a/lib/src/main/java/org/team1540/rooster/testers/motor/EncoderTester.java b/lib/src/main/java/org/team1540/rooster/testers/motor/EncoderTester.java new file mode 100644 index 00000000..9a2b2559 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/testers/motor/EncoderTester.java @@ -0,0 +1,145 @@ +package org.team1540.rooster.testers.motor; + +import com.ctre.phoenix.motorcontrol.IMotorController; +import edu.wpi.first.wpilibj.Sendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; +import java.util.Arrays; +import java.util.Collections; +import java.util.List; +import java.util.Optional; +import org.team1540.rooster.testers.AbstractTester; +import org.team1540.rooster.testers.ResultWithMetadata; + +/** + * Reports if an encoder appears to be non-functional by checking to see if a motor is running + * and if the corresponding encoder is moving. + */ +@SuppressWarnings("unused") +public class EncoderTester extends AbstractTester implements Sendable { + + private String name = "EncoderTester"; + + private double currentThreshold = 1; + private double velocityThreshold = 5; + + /** + * Construct a new instance with the default logTime of 150 seconds and an update delay of 500 + * ms. Equivalent to {@link EncoderTester#EncoderTester(List) EncoderTester(Arrays.asList + * (motorsToTest))}. + * + * @param motorsToTest The {@link IMotorController IMotorControllers} to compare to each other. + */ + @SuppressWarnings("WeakerAccess") + public EncoderTester(IMotorController... motorsToTest) { + this(Arrays.asList(motorsToTest)); + } + + /** + * Construct a new instance with the default logTime of 150 seconds and an update delay of 500 ms. + * + * @param motorsToTest The {@link IMotorController IMotorControllers} to compare to each other. + */ + @SuppressWarnings("WeakerAccess") + public EncoderTester(List motorsToTest) { + // Because passing in a reference to a non-static method in the constructor doesn't work. + // Test will run if the motor is drawing over 1A of current. + super((stupid) -> null, motorsToTest, null, 150, 500); + this.setTest(this::testEncoder); + this.setRunConditions( + Collections.singletonList((motor) -> (motor).getOutputCurrent() > currentThreshold)); + } + + /** + * Tests to see if the encoder is working by checking to see if the controller is drawing more + * than 1 amp and if the selected {@link IMotorController} is moving at a velocity of less than 5. + * + * @param controller The {@link IMotorController} to test for burnout. + * @return Boolean indicating if the encoder is encoder has failed: true if it is suspected + * of failure, false if it is not suspected of failure. + */ + @SuppressWarnings("WeakerAccess") + public Boolean testEncoder(IMotorController controller) { + // Do the wrappers provide pidIdx nicely? Yes. Can we just use zero? Also probably yes. + return controller.getOutputCurrent() > currentThreshold + && Math.abs(controller.getSelectedSensorVelocity(0)) < velocityThreshold; + } + + @Override + public String getName() { + return this.name; + } + + @Override + public void setName(String name) { + this.name = name; + } + + @Override + public String getSubsystem() { + return this.name; + } + + @Override + public void setSubsystem(String subsystem) { + this.name = subsystem; + } + + /** + * Gets the threshold under which a motor will not be tested for movement. Defaults to 1A. + * + * @return A double representing the current in amps. + */ + public double getCurrentThreshold() { + return currentThreshold; + } + + /** + * Sets the threshold under which a motor will not be tested for movement + * + * @param currentThreshold A double representing the current in amps. + */ + public void setCurrentThreshold(double currentThreshold) { + this.currentThreshold = currentThreshold; + } + + /** + * Gets the encoder velocity under which an encody failure will be reported. Defaults to 5. + * + * @return A double representing the velocity in whatever units are set. + */ + public double getVelocityThreshold() { + return velocityThreshold; + } + + /** + * Sets the encoder velocity under which an encody failure will be reported. + * + * @param velocityThreshold A double representing the velocity in whatever units are set. + */ + public void setVelocityThreshold(double velocityThreshold) { + this.velocityThreshold = velocityThreshold; + } + + /** + * Displays the current status of each {@link IMotorController}. + * + * @param builder The {@link SendableBuilder} to use. + */ + @Override + public void initSendable(SendableBuilder builder) { + //noinspection Duplicates + for (IMotorController t : getItemsToTest()) { + // Get the most recent value if present, else simply don't add it to the builder + //noinspection Duplicates + builder.addBooleanProperty(t.getDeviceID() + "", () -> { + // TODO probably cleaner version of this, at the least ifPresentOrElse() in Java 9 + Optional> result = Optional.ofNullable(peekMostRecentResult(t)); + if (result.isPresent()) { + return result.get().getResult(); + } else { + return false; + } + }, null); + } + } +} diff --git a/lib/src/main/java/org/team1540/rooster/testers/SimpleControllersTester.java b/lib/src/main/java/org/team1540/rooster/testers/motor/SimpleControllersTester.java similarity index 97% rename from lib/src/main/java/org/team1540/rooster/testers/SimpleControllersTester.java rename to lib/src/main/java/org/team1540/rooster/testers/motor/SimpleControllersTester.java index e01226f7..95cc63d0 100644 --- a/lib/src/main/java/org/team1540/rooster/testers/SimpleControllersTester.java +++ b/lib/src/main/java/org/team1540/rooster/testers/motor/SimpleControllersTester.java @@ -1,7 +1,8 @@ -package org.team1540.rooster.testers; +package org.team1540.rooster.testers.motor; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.IMotorController; +import com.ctre.phoenix.motorcontrol.NeutralMode; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.Sendable; import edu.wpi.first.wpilibj.buttons.JoystickButton; @@ -86,11 +87,11 @@ public SimpleControllersTester(Joystick joystick, int axisId, int nextButtonId, this.controllers.put(controller.getDeviceID(), controller); this.controllerChooser.addObject(String.valueOf(controller.getDeviceID()), controller.getDeviceID()); + controller.setNeutralMode(NeutralMode.Coast); } // Set the active controller to the first controller setCurrentController(controllers[0].getDeviceID()); - } /**