diff --git a/.gitignore b/.gitignore index cc3fb06a..5e05dc45 100644 --- a/.gitignore +++ b/.gitignore @@ -162,3 +162,6 @@ gradle-app.setting # motion profiles *.csv + +# zack's discord plugin +.idea/discord.xml diff --git a/README.md b/README.md index 1a11a187..22f8de01 100644 --- a/README.md +++ b/README.md @@ -12,40 +12,18 @@ A common library of useful classes and systems intended to be used for all Team [Javadoc hosted on Github Pages](https://flamingchickens1540.github.io/ROOSTER) -#### Drive Code - -`org.team1540.rooster.drive` - -Advanced closed-loop drive code for a tank drive. - #### Drive Pipeline System `org.team1540.rooster.drive.pipeline` A flexible system for controlling a robot drive. More docs [here](docs/Drive%20Pipelines.md), with a specific section on motion profiling [here](docs/Motion%20Profiling.md). -#### Power Management - -`org.team1540.rooster.power` - -A flexible, dynamic power management system. Uses a centralized `PowerManager` that takes `PowerManageable`s, including the default implementation `ChickenSubsystem`. - #### Preferences `org.team1540.rooster.preferencemanager` A system to easily set tuning fields through WPILib `Preferences`. -#### Testing - -`org.team1540.rooster.testers` - -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 `org.team1540.rooster.triggers` @@ -54,7 +32,6 @@ Simple triggers that extend WPILib's joystick binding functionality. - `AxisButton` allows using a joystick axis (triggers or joysticks) as a button–the button will trigger when the axis passes a user-defined threshold. - `DPadButton` and `StrictDPadButton` allow using any axis of a controller D-Pad as a button. -- `SimpleButton` allows using a generic `BooleanSupplier` as a button. #### Utilities @@ -66,15 +43,6 @@ Functions and classes for common tasks. - Capping an output - Inverting an input/output depending on a boolean -#### Utility Classes - -`org.team1540.rooster.util` - -Classes (mostly WPILib `Commands`) to make life easier. - -- `AsyncCommand` to execute a command in a separate thread on a loop. -- `SimpleCommand`/`SimpleLoopCommand` to quickly create a one-shot or loop command from a lambda. -- `SimpleConditionalCommand` to quickly create a `ConditionalCommand` from a lambda. ### Installation diff --git a/build.gradle b/build.gradle index afc61271..54f9e0c1 100644 --- a/build.gradle +++ b/build.gradle @@ -3,10 +3,13 @@ import edu.wpi.first.gradlerio.GradleRIOPlugin plugins { id "java" id "maven" - id "edu.wpi.first.GradleRIO" version "2019.1.1" - id "org.jetbrains.kotlin.jvm" version "1.3.0" + id "edu.wpi.first.GradleRIO" version "2020.1.1" + id "org.jetbrains.kotlin.jvm" version "1.3.50" + id "com.palantir.git-version" version "0.12.2" } +group "org.team1540" +version gitVersion() // Maven/Jitpack configuration task sourcesJar(type: Jar, dependsOn: classes) { @@ -19,6 +22,13 @@ task javadocJar(type: Jar, dependsOn: javadoc) { from javadoc.destinationDir } + +install { + doLast { + System.out.println("Installing with version " + gitVersion()) + } +} + artifacts { archives sourcesJar archives javadocJar @@ -47,14 +57,20 @@ repositories { dependencies { // FRC dependencies - compile wpi.deps.wpilib() - compile wpi.deps.vendor.java() + implementation wpi.deps.wpilib() + nativeZip wpi.deps.wpilibJni(wpi.platforms.roborio) + nativeDesktopZip wpi.deps.wpilibJni(wpi.platforms.desktop) + + implementation wpi.deps.vendor.java() nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio) nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop) // Non-FRC dependencies compile 'org.jetbrains:annotations:16.0.3' - compile "org.jetbrains.kotlin:kotlin-stdlib-jdk8:1.3.0" + compile "org.jetbrains.kotlin:kotlin-stdlib-jdk8:1.3.50" + + compile 'org.slf4j:slf4j-log4j12:1.6.1' + compile 'log4j:log4j:1.2.16' compile "com.google.guava:guava:27.0-jre" compile "org.apache.commons:commons-math3:3.6.1" @@ -73,7 +89,7 @@ sourceSets { } configurations { - testbotsCompile.extendsFrom compile + testbotsImplementation.extendsFrom implementation testbotsRuntimeOnly.extendsFrom runtimeOnly } @@ -139,13 +155,9 @@ jar { } } -simulateJava.dependsOn.removeAll() -simulateJava.dependsOn(extractTestJNI) -simulateJava.dependsOn(jar) - // wrapper wrapper { - gradleVersion = '5.0' + gradleVersion = '6.0.1' distributionType = Wrapper.DistributionType.ALL } diff --git a/src/main/java/org/team1540/rooster/ChickenSubsystem.java b/src/main/java/org/team1540/rooster/ChickenSubsystem.java deleted file mode 100644 index f7140b23..00000000 --- a/src/main/java/org/team1540/rooster/ChickenSubsystem.java +++ /dev/null @@ -1,382 +0,0 @@ -package org.team1540.rooster; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; -import java.util.Arrays; -import java.util.Collection; -import java.util.Map; -import java.util.Optional; -import java.util.concurrent.ConcurrentHashMap; -import java.util.concurrent.atomic.AtomicLong; -import org.jetbrains.annotations.NotNull; -import org.team1540.rooster.power.PowerManageable; -import org.team1540.rooster.power.PowerManager; -import org.team1540.rooster.power.PowerTelemetry; -import org.team1540.rooster.wrappers.ChickenController; -import org.team1540.rooster.wrappers.ChickenTalon; -import org.team1540.rooster.wrappers.ChickenVictor; - - -/** - * Simple implementation of core {@link Subsystem} related interfaces. Makes it quick and easy to - * build a basic robot. - */ -@SuppressWarnings("unused") -public class ChickenSubsystem extends Subsystem implements PowerManageable { - - /** - * Map of motors in this subsystem to be power managed, with the key being the motor and the value - * being the current percentOutput the motor is at. - */ - private final Map motors = new ConcurrentHashMap<>(); - private final PowerTelemetry allMotorTelemetry = new PowerTelemetry() { - @Override - public double getCurrent() { - double sum = 0; - for (ChickenController motor : motors.keySet()) { - if (motor instanceof ChickenTalon) { - sum += ((ChickenTalon) motor).getOutputCurrent(); - } - } - return sum; - } - - @Override - public double getVoltage() { - double sum = 0; - for (ChickenController motor : motors.keySet()) { - sum += motor.getMotorOutputVoltage(); - } - return sum / motors.size(); - } - }; - private double noiseThreshold = 0.25; - private double priority = 1.0; - private boolean telemetryCacheValid = false; - private PowerTelemetry telemetry = allMotorTelemetry; - - public ChickenSubsystem(String name) { - super(name); - realConstructor(); - } - - public ChickenSubsystem() { - super(); - realConstructor(); - } - - public int size() { - return motors.size(); - } - - public boolean isEmpty() { - return motors.isEmpty(); - } - - public boolean contains(@NotNull ChickenController o) { - return motors.containsKey(o); - } - - public void add(@NotNull ChickenController o) { - invalidateTelemetryCache(); - motors.put(o, new MotorProperties()); - } - - public void add(@NotNull ChickenController... os) { - addAll(Arrays.asList(os)); - } - - public void remove(@NotNull ChickenController o) { - invalidateTelemetryCache(); - motors.remove(o); - } - - public void remove(@NotNull ChickenController... os) { - removeAll(Arrays.asList(os)); - } - - public boolean containsAll(@NotNull Collection controllers) { - for (ChickenController c : controllers) { - if (!contains(c)) { - return false; - } - } - return true; - } - - public void addAll(@NotNull Collection controllers) { - for (ChickenController c : controllers) { - add(c); - } - } - - public void removeAll(@NotNull Collection controllers) { - for (ChickenController c : controllers) { - remove(c); - } - } - - public void clear() { - invalidateTelemetryCache(); - motors.clear(); - } - - private void realConstructor() { - PowerManager.getInstance().registerPowerManageable(this); - } - - @Override - protected void initDefaultCommand() { - - } - - @Override - public double getPriority() { - return priority; - } - - @Override - public void setPriority(double priority) { - this.priority = priority; - } - - @Override - public double getPercentOutputLimit() { - double sum = 0; - for (ChickenController currentMotor : motors.keySet()) { - // Use the lower bounds because I think it's nice. - sum += Math.min(Math.abs(currentMotor.getPeakOutputForward()), Math.abs(currentMotor - .getPeakOutputReverse())); - } - return sum / motors.size(); - } - - @Override - public double setRelativePercentOutputLimit(double limit) { - double overflow = 0; - for (Map.Entry entry : motors.entrySet()) { - if (Double.isNaN(limit)) { - DriverStation.reportError(this.getName() + ": Cannot set percentOutputLimit to " + limit - + " for motor " + entry.getKey() + ", passing", false); - return 0; - } - - final double newLimit = entry.getKey().getMotorOutputPercent() * limit; - - double forwardNewLimit = newLimit; - double reverseNewLimit = newLimit; - double forwardCeiling = entry.getValue().getAbsolutePeakOutputCeilingForward(); - double reverseCeiling = entry.getValue().getAbsolutePeakOutputCeilingReverse(); - - if (forwardNewLimit > forwardCeiling) { - forwardNewLimit = forwardCeiling; - overflow += forwardNewLimit - forwardCeiling; - } - if (reverseNewLimit > reverseCeiling) { - reverseNewLimit = reverseCeiling; - overflow += reverseNewLimit - reverseCeiling; - } - - // If the new limit is below the threshold, introduce some noise to keep it from being - // stuck at 0 - if (forwardNewLimit < noiseThreshold) { - forwardNewLimit = Math.random() * noiseThreshold; - } - if (reverseNewLimit < noiseThreshold) { - reverseNewLimit = Math.random() * noiseThreshold; - } - - entry.getKey().configPeakOutputForward(forwardNewLimit); - entry.getKey().configPeakOutputReverse(-reverseNewLimit); - } - // Average overflow (note the divide by two because of the forward and backwards) - // Also divide by two to account for forwards and reverse. It's not perfect, but it works. - // It's not perfect, but it should be good enough - return overflow / motors.size() / 2; - } - - @Override - public void stopLimitingPower() { - for (Map.Entry entry : motors.entrySet()) { - final double ceiling = entry.getValue().getAbsolutePeakOutputCeilingForward(); - entry.getKey().configPeakOutputForward(ceiling); - entry.getKey().configPeakOutputReverse(-ceiling); - } - } - - public boolean isTelemetryCacheValid() { - return telemetryCacheValid; - } - - public void invalidateTelemetryCache() { - telemetryCacheValid = false; - } - - private void setTelemetry(PowerTelemetry t) { - telemetry = t; - telemetryCacheValid = true; - } - - /** - * Gets the threshold below which the output will be randomized to prevent the output from - * being stuck at 0. - * - * @return A float between 0 and 1 inclusive. - */ - public double getNoiseThreshold() { - return noiseThreshold; - } - - /** - * Gets the threshold below which the output will be randomized to prevent the output from - * being stuck at 0. - * - * @param noiseThreshold A float between 0 and 1 inclusive. - */ - public void setNoiseThreshold(double noiseThreshold) { - if (noiseThreshold < 0 || noiseThreshold > 1) { - throw new IllegalArgumentException("noiseThreshold must be between 0 and 1 inclusive, got " - + noiseThreshold); - } - this.noiseThreshold = noiseThreshold; - } - - - /** - * Returns an object that gives the aggregate data from {@link ChickenTalon}s if all motors are - * either {@link ChickenTalon}s or slaved {@link ChickenVictor}s. - * Else, returns null. - * Basically, either the entire subsystem has telemetry or none of it does. - * Note that to improve performance, this status is cached and only updated when either a motor - * is added, removed, or the cache is externally invalidaded using - * {@link #invalidateTelemetryCache()}. - * - * Override me as necessary (e.g. for all-Victor subsystems where you'd be getting the - * telemetry from the PDP.) - * - * @return The according {@link PowerTelemetry} object. - */ - @Override - public Optional getPowerTelemetry() { - if (telemetryCacheValid) { - return Optional.ofNullable(telemetry); - } else { - // This unforunately needs to be checked at runtime, as if a Victor is slaved can change - // really at any time - for (ChickenController currentMotor : motors.keySet()) { - if (currentMotor instanceof ChickenVictor && !currentMotor.getControlMode().equals - (ControlMode.Follower)) { - setTelemetry(null); - return Optional.empty(); - } - } - setTelemetry(allMotorTelemetry); - return Optional.of(allMotorTelemetry); - } - } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - sendablePowerInfo(builder); - builder.addBooleanProperty("telemetryCacheValid", this::isTelemetryCacheValid, null); - builder.addDoubleProperty("noiseThreshold", this::getNoiseThreshold, this::setNoiseThreshold); - } - - /** - * Gets the maximum forward peak output. - * - * @param m The motor to get the maximum forward peak output of. - * @return Double 0 to 1 inclusive (always positive) - */ - public double getAbsolutePeakOutputCeilingForward(ChickenController m) { - return motors.get(m).getAbsolutePeakOutputCeilingForward(); - } - - /** - * Sets the maximum forward peak output. - * @param m The motor to get the maximum forward peak output of. - * @param absolutePeakOutputCeiling Double -1 to 1 inclusive. Negative numbers are made positive. - */ - public void setAbsolutePeakOutputCeilingForward(ChickenController m, - double absolutePeakOutputCeiling) { - motors.get(m).setAbsolutePeakOutputCeilingForward(absolutePeakOutputCeiling); - // If the power management is running, we can just wait until its next tick deals with the - // ceilings. However, if it's not, we'll need to deal with the ceilings ourselves. - if (!PowerManager.getInstance().isLimiting()) { - m.configPeakOutputForward(Math.abs(absolutePeakOutputCeiling)); - } - } - - /** - * Gets the maximum reverse peak output. - * @param m The motor to get the maximum reverse peak output of. - * @return Double 0 to 1 inclusive (always positive) - */ - public double getAbsolutePeakOutputCeilingReverse(ChickenController m) { - return motors.get(m).getAbsolutePeakOutputCeilingReverse(); - } - - /** - * Sets the maximum reverse peak output. - * @param m The motor to get the maximum reverse peak output of. - * @param absolutePeakOutputCeiling Double -1 to 1 inclusive. Negative numbers are made positive. - */ - public void setAbsolutePeakOutputCeilingReverse(ChickenController m, - double absolutePeakOutputCeiling) { - motors.get(m).setAbsolutePeakOutputCeilingReverse(absolutePeakOutputCeiling); - // If the power management is running, we can just wait until its next tick deals with the - // ceilings. However, if it's not, we'll need to deal with the ceilings ourselves. - if (!PowerManager.getInstance().isLimiting()) { - m.configPeakOutputReverse(-Math.abs(absolutePeakOutputCeiling)); - } - } - - private class MotorProperties { - - private final AtomicLong absolutePeakOutputCeilingForward = new AtomicLong(Double - .doubleToLongBits(1.0)); - private final AtomicLong absolutePeakOutputCeilingReverse = new AtomicLong(Double - .doubleToLongBits(1.0)); - - /** - * Gets the maximum forward peak output. - * @return Double 0 to 1 inclusive (always positive) - */ - public double getAbsolutePeakOutputCeilingForward() { - return Double.longBitsToDouble(absolutePeakOutputCeilingForward.get()); - } - - /** - * Sets the maximum forward peak output. - * @param absolutePeakOutputCeilingForward Double -1 to 1 inclusive. Negative numbers are - * made positive. - */ - public void setAbsolutePeakOutputCeilingForward(double absolutePeakOutputCeilingForward) { - this.absolutePeakOutputCeilingForward - .set(Double.doubleToLongBits(absolutePeakOutputCeilingForward)); - } - - /** - * Gets the maximum reverse peak output. - * @return Double 0 to 1 inclusive (always positive) - */ - public double getAbsolutePeakOutputCeilingReverse() { - return Double.longBitsToDouble(absolutePeakOutputCeilingReverse.get()); - } - - /** - * Sets the maximum reverse peak output. - * @param absolutePeakOutputCeilingReverse Double -1 to 1 inclusive. Negative numbers are - * made positive. - */ - public void setAbsolutePeakOutputCeilingReverse(double absolutePeakOutputCeilingReverse) { - this.absolutePeakOutputCeilingReverse - .set(Double.doubleToLongBits(absolutePeakOutputCeilingReverse)); - } - - } - -} diff --git a/src/main/java/org/team1540/rooster/Utilities.java b/src/main/java/org/team1540/rooster/Utilities.java deleted file mode 100644 index a91a76d5..00000000 --- a/src/main/java/org/team1540/rooster/Utilities.java +++ /dev/null @@ -1,120 +0,0 @@ -package org.team1540.rooster; - -import org.jetbrains.annotations.Contract; - -/** - * Static utility functions. - */ -public class Utilities { - - /** - * Processes an axis and returns the value only if it is outside the provided deadzone. - * - * @param axis The axis to return. - * @param deadzone The deadzone to use. - * @return If |{@code axis}| is greater than |{@code deadzone}|, returns {@code axis}; otherwise, - * returns 0. - * @deprecated Use {@link #processDeadzone(double, double) processDeadzone()}'s improved - * algorithm. - */ - @Deprecated - public static double processAxisDeadzone(double axis, double deadzone) { - return (Math.abs(axis) > Math.abs(deadzone)) ? axis : 0; - } - - /** - * Processes an axis with a deadzone. This implementation scales the output such that the area - * between the deadzone and 1 is mapped to the full range of motion. - *

- * Full implementation details: The function processes the deadzone according to the function - * \(\DeclareMathOperator{\sgn}{sgn}\frac{d(x)-Z\sgn(d(x))}{1-Z}\), where \(x\) is the {@code - * axis} parameter, \(Z\) is the {@code deadzone} parameter, and \(d(x)\) is the deadzone function - * \(d(x)=\begin{cases}x & |x| \geq D \\ 0 & |x| < D\end{cases}\). (Note: Equations - * may require viewing the Javadoc using a browser.) - * - * @param axis The axis to return. - * @param deadzone The deadzone to use. - * @return The axis value processed with respect to the specified deadzone. - */ - @Contract(pure = true) - public static double processDeadzone(double axis, double deadzone) { - double baseDeadzone = (Math.abs(axis) > Math.abs(deadzone)) ? axis : 0; - return baseDeadzone != 0 ? (baseDeadzone - Math.copySign(deadzone, baseDeadzone)) / (1 - - deadzone) : 0; - } - - /** - * Inverts the provided value if {@code shouldInvert} is {@code true}. - * - * @param shouldInvert Whether to invert the number. - * @param toInvert The number to invert. - * @return If {@code shouldInvert} is {@code true}, {@code -toInvert}; otherwise, {@code toInvert} - */ - @Contract(pure = true) - public static double invertIf(boolean shouldInvert, double toInvert) { - return (shouldInvert ? -1 : 1) * toInvert; - } - - /** - * Constrains an input to a given range in either direction from zero. - * - * This does not map the input to the range; it simply hard-caps it when it's - * outside. - * - * @param input The input to constrain. - * @param cap The distance from zero to constrain {@code input} to. - * @return If {@code input} > {@code cap}, return {@code cap}; if {@code input} < {@code - * -cap}, return {@code -cap}; otherwise, return {@code input}. - */ - @Contract(pure = true) - public static double constrain(double input, double cap) { - if (cap < 0) { - throw new IllegalArgumentException("Cap cannot be negative"); - } - - return constrain(input, -cap, cap); - } - - /** - * Constrains an input to a given range. - * - * This does not map the input to the range; it simply hard-caps it when it's - * outside. - * - * @param input The input to constrain. - * @param lowerCap The lower bound of the range. - * @param upperCap The upper bound of the range. - * @return If {@code input} > {@code upperCap}, return {@code upperCap}; if {@code input} < - * {@code lowerCap}, return {@code lowerCap}; otherwise, return {@code input}. - */ - @Contract(pure = true) - public static double constrain(double input, double lowerCap, double upperCap) { - if (lowerCap > upperCap) { - throw new IllegalArgumentException("Lower cap cannot be less than upper cap"); - } - - if (input < lowerCap) { - return lowerCap; - } else if (input > upperCap) { - return upperCap; - } else { - return input; - } - } - - /** - * Raises the input to the provided power while preserving the sign. Useful for joystick scaling. - * - * @param input The input to be raised. - * @param pow The power. - * @return The input raised to the provided power, with the sign of the input. - */ - @Contract(pure = true) - public static double scale(double input, double pow) { - return Math.copySign(Math.pow(Math.abs(input), pow), input); - } - - // should never be instantiated - private Utilities() { - } -} diff --git a/src/main/java/org/team1540/rooster/adjustables/AdjustableManager.java b/src/main/java/org/team1540/rooster/adjustables/AdjustableManager.java deleted file mode 100644 index af989431..00000000 --- a/src/main/java/org/team1540/rooster/adjustables/AdjustableManager.java +++ /dev/null @@ -1,195 +0,0 @@ -package org.team1540.rooster.adjustables; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import java.lang.reflect.Field; -import java.util.LinkedList; -import java.util.List; -import org.team1540.rooster.util.SimpleLoopCommand; - -/** - * Class to manage creating and updating adjustables (tunables and telemetry values.) Add an object - * containing fields marked with {@link Tunable} or {@link Telemetry} to have those values show up - * on the SmartDashboard/Shuffleboard. - * - * @deprecated Use the {@link org.team1540.rooster.preferencemanager.PreferenceManager}. - */ -@Deprecated -public class AdjustableManager { - - private static AdjustableManager instance = new AdjustableManager(); - - private final Object lock = new Object(); - private List tunables = new LinkedList<>(); - private List telemetry = new LinkedList<>(); - - private boolean enabled = true; - - private AdjustableManager() { - SimpleLoopCommand managerUpdate = new SimpleLoopCommand("AdjustableManager Update", - this::run); - managerUpdate.setRunWhenDisabled(true); - managerUpdate.start(); - } - - @Deprecated - public static AdjustableManager getInstance() { - return instance; - } - - /** - * Adds an object to the {@code AdjustableManager}. - * - * @param object The object to add. One or more of the fields in this object should be marked with - * {@link Telemetry} or {@link Tunable}. - * @deprecated Use the {@link org.team1540.rooster.preferencemanager.PreferenceManager}. - */ - @Deprecated - public void add(Object object) { - synchronized (lock) { - // reflection time - Field[] fields = object.getClass().getFields(); - - boolean noneFound = true; // for logging to keep track if we have found at least one adjustable - for (Field field : fields) { - - // process tunables - Tunable tunable = field.getAnnotation(Tunable.class); - - if (tunable != null) { - // check if the field is of a supported type - TunableType tt = null; - for (TunableType type : TunableType.values()) { - //noinspection unchecked - if (type.cls.isAssignableFrom(field.getType())) { - tt = type; - break; - } - } - - if (tt == null) { - DriverStation.reportError( - "Annotated tunable in class added to AdjustableManager is not of a supported type", - false); - continue; - } - - tunables.add(new TunableField(object, field, tt, tunable.value())); - noneFound = false; - } - - // process telemetry - Telemetry teleAnnotation = field.getAnnotation(Telemetry.class); - - if (teleAnnotation != null) { - // check if the field is of a supported type - TelemetryType tt = null; - for (TelemetryType type : TelemetryType.values()) { - //noinspection unchecked - if (type.cls.isAssignableFrom(field.getType())) { - tt = type; - break; - } - } - - if (tt == null) { - DriverStation.reportError( - "Annotated telemetry in class added to AdjustableManager is not of a supported type", - false); - continue; - } - - telemetry.add(new TelemetryField(object, field, tt, teleAnnotation.value())); - noneFound = false; - } - } - if (noneFound) { - DriverStation.reportWarning( - "Object passed to AdjustableManager had no annotated adjustable fields", - false); - } - } - } - - private void run() { - if (enabled) { - synchronized (lock) { - // Update tunables - for (TunableField tf : tunables) { - try { - if (!SmartDashboard.containsKey(tf.label)) { - //noinspection unchecked - tf.type.putFunction.put(tf.label, tf.field.get(tf.obj)); - } else { - //noinspection unchecked - tf.field.set(tf.obj, tf.type.getFunction.get(tf.label, tf.field.get(tf.obj))); - } - } catch (IllegalAccessException e) { - DriverStation.reportError(e.getMessage(), true); - } - } - - // Update telemetry - for (TelemetryField tf : telemetry) { - try { - //noinspection unchecked - tf.type.putFunction.put(tf.label, tf.field.get(tf.obj)); - } catch (IllegalAccessException e) { - DriverStation.reportError(e.toString(), true); - } - } - } - } - } - - /** - * Updates adjustable values. This method should be called in {@code robotPeriodic()} in your main - * {@code Robot} class. - * - * @deprecated No longer necessary; now updates automatically. - */ - @Deprecated - public void update() { - run(); - } - - private static class TunableField { - - Object obj; - Field field; - TunableType type; - String label; - - public TunableField(Object obj, Field field, TunableType type, String label) { - this.obj = obj; - this.field = field; - this.type = type; - this.label = label; - } - } - - private static class TelemetryField { - - Object obj; - Field field; - TelemetryType type; - String label; - - public TelemetryField(Object obj, Field field, TelemetryType type, String label) { - this.obj = obj; - this.field = field; - this.type = type; - this.label = label; - } - } - - @Deprecated - public boolean isEnabled() { - return enabled; - } - - @Deprecated - public void setEnabled(boolean enabled) { - this.enabled = enabled; - } -} diff --git a/src/main/java/org/team1540/rooster/adjustables/SmartDashboardGet.java b/src/main/java/org/team1540/rooster/adjustables/SmartDashboardGet.java deleted file mode 100644 index c6a7320f..00000000 --- a/src/main/java/org/team1540/rooster/adjustables/SmartDashboardGet.java +++ /dev/null @@ -1,7 +0,0 @@ -package org.team1540.rooster.adjustables; - -@FunctionalInterface -@Deprecated -interface SmartDashboardGet { - T get(String key, T dfault); -} diff --git a/src/main/java/org/team1540/rooster/adjustables/SmartDashboardPut.java b/src/main/java/org/team1540/rooster/adjustables/SmartDashboardPut.java deleted file mode 100644 index 57c13f69..00000000 --- a/src/main/java/org/team1540/rooster/adjustables/SmartDashboardPut.java +++ /dev/null @@ -1,7 +0,0 @@ -package org.team1540.rooster.adjustables; - -@FunctionalInterface -@Deprecated -interface SmartDashboardPut { - void put(String key, T value); -} diff --git a/src/main/java/org/team1540/rooster/adjustables/Telemetry.java b/src/main/java/org/team1540/rooster/adjustables/Telemetry.java deleted file mode 100644 index c3ada100..00000000 --- a/src/main/java/org/team1540/rooster/adjustables/Telemetry.java +++ /dev/null @@ -1,24 +0,0 @@ -package org.team1540.rooster.adjustables; - -import static java.lang.annotation.ElementType.FIELD; -import static java.lang.annotation.RetentionPolicy.RUNTIME; - -import java.lang.annotation.Retention; -import java.lang.annotation.Target; - -/** - * Indicates that a field's value should be shown on the SmartDashboard/Shuffleboard. This field can - * be applied to fields of the following types:

  • int
  • double
  • String
    • - *
  • Boolean
  • Solenoid
  • SpeedController
  • Accelerometer
  • Gyro
    • - *
  • Potentiometer
  • CounterBase
- * - * @deprecated Use the {@link org.team1540.rooster.preferencemanager.PreferenceManager} and {@link - * org.team1540.rooster.preferencemanager.Preference} instead. - */ -@Retention(RUNTIME) -@Target(FIELD) -@Deprecated -public @interface Telemetry { - - String value(); -} diff --git a/src/main/java/org/team1540/rooster/adjustables/TelemetryType.java b/src/main/java/org/team1540/rooster/adjustables/TelemetryType.java deleted file mode 100644 index b6aeee2d..00000000 --- a/src/main/java/org/team1540/rooster/adjustables/TelemetryType.java +++ /dev/null @@ -1,21 +0,0 @@ -package org.team1540.rooster.adjustables; - -import edu.wpi.first.wpilibj.Sendable; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -@Deprecated -enum TelemetryType { - STRING(String.class, SmartDashboard::putString), - INT(Integer.TYPE, (SmartDashboardPut) SmartDashboard::putNumber), - DOUBLE(Double.TYPE, SmartDashboard::putNumber), - BOOLEAN(Boolean.TYPE, SmartDashboard::putBoolean), - SENDABLE(Sendable.class, SmartDashboard::putData); - - final Class cls; - final SmartDashboardPut putFunction; - - TelemetryType(Class cls, SmartDashboardPut putFunction) { - this.cls = cls; - this.putFunction = putFunction; - } -} diff --git a/src/main/java/org/team1540/rooster/adjustables/Tunable.java b/src/main/java/org/team1540/rooster/adjustables/Tunable.java deleted file mode 100644 index 33c38540..00000000 --- a/src/main/java/org/team1540/rooster/adjustables/Tunable.java +++ /dev/null @@ -1,28 +0,0 @@ -package org.team1540.rooster.adjustables; - -import static java.lang.annotation.ElementType.FIELD; -import static java.lang.annotation.RetentionPolicy.RUNTIME; - -import java.lang.annotation.Retention; -import java.lang.annotation.Target; - -/** - * Indicates that a field should be tunable from the SmartDashboard/Shuffleboard. Changes on the - * SmartDashboard/Shuffleboard will be reflected in this field, and as such it's useful for fields - * like PID tuning values. - * - * @deprecated Use the {@link org.team1540.rooster.preferencemanager.PreferenceManager} and {@link - * org.team1540.rooster.preferencemanager.Preference} instead. - */ -@Retention(RUNTIME) -@Target(FIELD) -@Deprecated -public @interface Tunable { - - /** - * Label for the entry on the SmartDashboard/Shuffleboard. - * - * @return the SmartDashboard/Shuffleboard label. - */ - String value(); -} diff --git a/src/main/java/org/team1540/rooster/adjustables/TunableType.java b/src/main/java/org/team1540/rooster/adjustables/TunableType.java deleted file mode 100644 index 1236f8f7..00000000 --- a/src/main/java/org/team1540/rooster/adjustables/TunableType.java +++ /dev/null @@ -1,22 +0,0 @@ -package org.team1540.rooster.adjustables; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -@Deprecated -enum TunableType { - STRING(String.class, SmartDashboard::putString, SmartDashboard::getString), - INT(Integer.TYPE, (SmartDashboardPut) SmartDashboard::putNumber, - (key, defaultValue) -> (int) SmartDashboard.getNumber(key, defaultValue)), - DOUBLE(Double.TYPE, SmartDashboard::putNumber, SmartDashboard::getNumber), - BOOLEAN(Boolean.TYPE, SmartDashboard::putBoolean, SmartDashboard::getBoolean); - - final Class cls; - final SmartDashboardGet getFunction; - final SmartDashboardPut putFunction; - - TunableType(Class cls, SmartDashboardPut putFunction, SmartDashboardGet getFunction) { - this.cls = cls; - this.putFunction = putFunction; - this.getFunction = getFunction; - } -} diff --git a/src/main/java/org/team1540/rooster/datastructures/Odometry.java b/src/main/java/org/team1540/rooster/datastructures/Odometry.java new file mode 100644 index 00000000..c7bbd17e --- /dev/null +++ b/src/main/java/org/team1540/rooster/datastructures/Odometry.java @@ -0,0 +1,26 @@ +package org.team1540.rooster.datastructures; + + +import org.team1540.rooster.datastructures.threed.Transform3D; +import org.team1540.rooster.datastructures.twod.Twist2D; + +public class Odometry { + + public static final Odometry IDENTITY = new Odometry(Transform3D.IDENTITY, Twist2D.ZERO); + + private final Transform3D pose; + private final Twist2D twist; + + public Odometry(Transform3D pose, Twist2D twist) { + this.pose = pose; + this.twist = twist; + } + + public Transform3D getPose() { + return pose; + } + + public Twist2D getTwist() { + return twist; + } +} diff --git a/src/main/java/org/team1540/rooster/datastructures/threed/Transform3D.java b/src/main/java/org/team1540/rooster/datastructures/threed/Transform3D.java new file mode 100644 index 00000000..cb4625f0 --- /dev/null +++ b/src/main/java/org/team1540/rooster/datastructures/threed/Transform3D.java @@ -0,0 +1,60 @@ +package org.team1540.rooster.datastructures.threed; + + +import org.apache.commons.math3.geometry.euclidean.threed.Rotation; +import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; +import org.team1540.rooster.datastructures.twod.Transform2D; +import org.team1540.rooster.datastructures.utils.RotationUtils; + +public class Transform3D { + + public static final Transform3D IDENTITY = new Transform3D(Vector3D.ZERO, Rotation.IDENTITY); + + private final Vector3D position; + private final Rotation orientation; + + public Transform3D(Vector3D position, Rotation orientation) { + this.position = position; + this.orientation = orientation; + } + + public Transform3D(Vector3D position) { + this(position, Rotation.IDENTITY); + } + + public Transform3D(Rotation rotation) { + this(Vector3D.ZERO, rotation); + } + + public Transform3D(double x, double y, double z, double roll, double pitch, double yaw) { + this(new Vector3D(x, y, z), RotationUtils.fromRPY(roll, pitch, yaw)); + } + + public Transform3D(double x, double y, double yaw) { + this(x, y, 0, 0, 0, yaw); + } + + public Transform2D toTransform2D() { + return new Transform2D(this.position.getX(), this.position.getY(), RotationUtils.getRPYVec(this.orientation).getZ()); + } + + public Vector3D getPosition() { + return position; + } + + public Rotation getOrientation() { + return orientation; + } + + public Transform3D add(Transform3D other) { + return new Transform3D(this.orientation.applyInverseTo(other.position).add(this.position), this.orientation.applyTo(other.orientation)); + } + + public Transform3D subtract(Transform3D other) { + return add(other.negate()); + } + + public Transform3D negate() { + return new Transform3D(orientation.applyTo(position.negate()), orientation.revert()); + } +} diff --git a/src/main/java/org/team1540/rooster/datastructures/twod/Transform2D.java b/src/main/java/org/team1540/rooster/datastructures/twod/Transform2D.java new file mode 100644 index 00000000..bafabab3 --- /dev/null +++ b/src/main/java/org/team1540/rooster/datastructures/twod/Transform2D.java @@ -0,0 +1,73 @@ +package org.team1540.rooster.datastructures.twod; + + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; +import org.team1540.rooster.datastructures.threed.Transform3D; + +/** + * 2D pose data structure class + */ +public class Transform2D { + + public static final Transform2D ZERO = new Transform2D(0, 0, 0); + + private final double x; + private final double y; + private final double theta; + + /** + * @param x Distance in meters in the X direction + * @param y Distance in meters in the Y direction + * @param theta Angle in radians between -PI and PI + */ + public Transform2D(double x, double y, double theta) { + this.x = x; + this.y = y; + this.theta = theta; + } + + public double getX() { + return x; + } + + public double getY() { + return y; + } + + public double getTheta() { + return theta; + } + + public Vector2D getPositionVector() { + return new Vector2D(x, y); + } + + public Transform3D toTransform3D() { + return new Transform3D(x, y, theta); + } + + public Transform2D add(Transform2D other) { + Transform3D thisTransform3D = this.toTransform3D(); + Transform3D otherTransform3D = other.toTransform3D(); + Transform3D transform3D = new Transform3D(thisTransform3D.getOrientation().applyInverseTo(otherTransform3D.getPosition()).add(thisTransform3D.getPosition()), + thisTransform3D.getOrientation().applyTo(otherTransform3D.getOrientation())); + return transform3D.toTransform2D(); + } + + public Transform2D subtract(Transform2D other) { + Transform3D thisTransform3D = this.toTransform3D(); + Transform3D otherTransform3D = other.toTransform3D(); + Transform3D transform3D = new Transform3D(thisTransform3D.getPosition().subtract(thisTransform3D.getOrientation().applyInverseTo(otherTransform3D.getPosition())), + thisTransform3D.getOrientation().applyInverseTo(otherTransform3D.getOrientation())); + return transform3D.toTransform2D(); + } + + public void putToNetworkTable(String networkTablesPath) { + NetworkTable table = NetworkTableInstance.getDefault().getTable(networkTablesPath); + table.getEntry("position/x").setNumber(getX()); + table.getEntry("position/y").setNumber(getY()); + table.getEntry("orientation/z").setNumber(getTheta()); + } +} diff --git a/src/main/java/org/team1540/rooster/datastructures/twod/Twist2D.java b/src/main/java/org/team1540/rooster/datastructures/twod/Twist2D.java new file mode 100644 index 00000000..4c6c30be --- /dev/null +++ b/src/main/java/org/team1540/rooster/datastructures/twod/Twist2D.java @@ -0,0 +1,46 @@ +package org.team1540.rooster.datastructures.twod; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; + +/** + * 2D twist data structure class + */ +public class Twist2D { + + public static final Twist2D ZERO = new Twist2D(0, 0, 0); + + private final double x; + private final double y; + private final double omega; + + /** + * @param x Linear velocity in X direction in meters per second + * @param y Linear velocity in Y direction in meters per second + * @param omega Angular velocity counter-clockwise in radians per second + */ + public Twist2D(double x, double y, double omega) { + this.x = x; + this.y = y; + this.omega = omega; + } + + public double getX() { + return x; + } + + public double getY() { + return y; + } + + public double getOmega() { + return omega; + } + + public void putToNetworkTable(String networkTablesPath) { + NetworkTable table = NetworkTableInstance.getDefault().getTable(networkTablesPath); + table.getEntry("velocity/x").setNumber(getX()); + table.getEntry("velocity/y").setNumber(getY()); + table.getEntry("angular/z").setNumber(getOmega()); + } +} diff --git a/src/main/java/org/team1540/rooster/datastructures/utils/RotationUtils.java b/src/main/java/org/team1540/rooster/datastructures/utils/RotationUtils.java new file mode 100644 index 00000000..36b2bf82 --- /dev/null +++ b/src/main/java/org/team1540/rooster/datastructures/utils/RotationUtils.java @@ -0,0 +1,21 @@ +package org.team1540.rooster.datastructures.utils; + +import org.apache.commons.math3.geometry.euclidean.threed.Rotation; +import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention; +import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder; +import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; + +public class RotationUtils { + + public static final RotationOrder ROTATION_ORDER = RotationOrder.XYZ; + public static final RotationConvention ROTATION_CONVENTION = RotationConvention.FRAME_TRANSFORM; + + public static Vector3D getRPYVec(Rotation rot) { + double[] angles = rot.getAngles(ROTATION_ORDER, ROTATION_CONVENTION); + return new Vector3D(angles[0], angles[1], angles[2]); + } + + public static Rotation fromRPY(double roll, double pitch, double yaw) { + return new Rotation(ROTATION_ORDER, ROTATION_CONVENTION, roll, pitch, yaw); + } +} diff --git a/src/main/java/org/team1540/rooster/datastructures/utils/UnitsUtils.java b/src/main/java/org/team1540/rooster/datastructures/utils/UnitsUtils.java new file mode 100644 index 00000000..b64548d1 --- /dev/null +++ b/src/main/java/org/team1540/rooster/datastructures/utils/UnitsUtils.java @@ -0,0 +1,14 @@ +package org.team1540.rooster.datastructures.utils; + +public class UnitsUtils { + + private static final double INCHES_PER_METER = 39.3701; + + public static double inchesToMeters(double inches) { + return inches / INCHES_PER_METER; + } + + public static double metersToInches(double meters) { + return meters * INCHES_PER_METER; + } +} diff --git a/src/main/java/org/team1540/rooster/drive/JoystickScaling.java b/src/main/java/org/team1540/rooster/drive/JoystickScaling.java deleted file mode 100644 index 90ed813a..00000000 --- a/src/main/java/org/team1540/rooster/drive/JoystickScaling.java +++ /dev/null @@ -1,13 +0,0 @@ -package org.team1540.rooster.drive; - -import org.team1540.rooster.functional.Processor; - -@FunctionalInterface -public interface JoystickScaling extends Processor { - - double scale(double input); - - default Double apply(Double input) { - return scale(input); - } -} diff --git a/src/main/java/org/team1540/rooster/drive/NoJoystickScaling.java b/src/main/java/org/team1540/rooster/drive/NoJoystickScaling.java deleted file mode 100644 index fdd799c8..00000000 --- a/src/main/java/org/team1540/rooster/drive/NoJoystickScaling.java +++ /dev/null @@ -1,12 +0,0 @@ -package org.team1540.rooster.drive; - -/** - * {@code JoystickScaling} implementation that does nothing; just returns the input. - */ -public class NoJoystickScaling implements JoystickScaling { - - @Override - public double scale(double input) { - return input; - } -} diff --git a/src/main/java/org/team1540/rooster/drive/PidDrive.java b/src/main/java/org/team1540/rooster/drive/PidDrive.java deleted file mode 100644 index 8a3768c2..00000000 --- a/src/main/java/org/team1540/rooster/drive/PidDrive.java +++ /dev/null @@ -1,275 +0,0 @@ -package org.team1540.rooster.drive; - -import static java.lang.Math.abs; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.command.Command; -import java.util.Objects; -import org.team1540.rooster.ChickenSubsystem; -import org.team1540.rooster.Utilities; -import org.team1540.rooster.wrappers.ChickenController; - -public class PidDrive extends Command { - - private ChickenController left; - private ChickenController right; - - private double maxVel; - private JoystickScaling scaling; - private double maxBrakePct; - private boolean invertLeftBrakeDirection; - private boolean invertRightBrakeDirection; - private double brakingStopZone; - - private Joystick joystick; - private int leftAxis; - private boolean invertLeft; - private int rightAxis; - private boolean invertRight; - private int forwardTrigger; - private int backTrigger; - private double deadzone; - private double brakeOverrideThresh; - private TriConsumer configPeakOutput; - - PidDrive(PidDriveConfiguration pidDriveConfiguration) { - requires(pidDriveConfiguration.subsystem); - // If the subsystem is a ChickenSubsystem, use setRelativePercentOutputLimit - // If not, use the good old configPeakOutput - - configPeakOutput = pidDriveConfiguration.subsystem instanceof ChickenSubsystem ? - (motor, forward, reverse) -> { - ((ChickenSubsystem) pidDriveConfiguration.subsystem) - .setAbsolutePeakOutputCeilingForward(motor, forward); - ((ChickenSubsystem) pidDriveConfiguration.subsystem) - .setAbsolutePeakOutputCeilingReverse(motor, reverse); - } - : (motor, forward, reverse) -> { - motor.configPeakOutputForward(forward); - motor.configPeakOutputReverse(-Math.abs(reverse)); - }; - - this.left = pidDriveConfiguration.left; - this.right = pidDriveConfiguration.right; - this.maxVel = pidDriveConfiguration.maxVel; - this.scaling = pidDriveConfiguration.scaling; - this.maxBrakePct = pidDriveConfiguration.maxBrakePct; - this.invertLeftBrakeDirection = pidDriveConfiguration.invertLeftBrakeDirection; - this.invertRightBrakeDirection = pidDriveConfiguration.invertRightBrakeDirection; - this.brakingStopZone = pidDriveConfiguration.brakingStopZone; - this.joystick = pidDriveConfiguration.joystick; - this.leftAxis = pidDriveConfiguration.leftAxis; - this.invertLeft = pidDriveConfiguration.invertLeft; - this.rightAxis = pidDriveConfiguration.rightAxis; - this.invertRight = pidDriveConfiguration.invertRight; - this.forwardTrigger = pidDriveConfiguration.forwardTrigger; - this.backTrigger = pidDriveConfiguration.backTrigger; - this.deadzone = pidDriveConfiguration.deadzone; - this.brakeOverrideThresh = pidDriveConfiguration.brakeOverrideThresh; - } - - @Override - protected void initialize() { - } - - @Override - protected void execute() { - // inputs - double fwdTriggerInput = Utilities - .processDeadzone(joystick.getRawAxis(forwardTrigger), deadzone); - - double backTriggerInput = Utilities.processDeadzone(joystick.getRawAxis(backTrigger), deadzone); - - double triggerInput = fwdTriggerInput - backTriggerInput; - - double leftInput = Utilities - .invertIf(invertLeft, Utilities.processDeadzone(joystick.getRawAxis(leftAxis), deadzone)); - - double rightInput = Utilities - .invertIf(invertRight, Utilities.processDeadzone(joystick.getRawAxis(rightAxis), deadzone)); - - double leftSetpoint = Utilities.constrain(scaling.scale(leftInput + triggerInput), 1); - double rightSetpoint = Utilities.constrain(scaling.scale(rightInput + triggerInput), 1); - - doPeakOutput(left, leftSetpoint, invertLeftBrakeDirection); - doPeakOutput(right, rightSetpoint, invertRightBrakeDirection); - - left.set(ControlMode.Velocity, leftSetpoint * maxVel); - right.set(ControlMode.Velocity, rightSetpoint * maxVel); - } - - @Override - protected void end() { - } - - @Override - protected boolean isFinished() { - return false; - } - - public ChickenController getLeft() { - return left; - } - - public void setLeft(ChickenController left) { - this.left = left; - } - - public ChickenController getRight() { - return right; - } - - public void setRight(ChickenController right) { - this.right = right; - } - - public double getMaxVel() { - return maxVel; - } - - public void setMaxVel(double maxVel) { - this.maxVel = maxVel; - } - - public JoystickScaling getScaling() { - return scaling; - } - - public void setScaling(JoystickScaling scaling) { - this.scaling = scaling; - } - - public double getMaxBrakePct() { - return maxBrakePct; - } - - public void setMaxBrakePct(double maxBrakePct) { - this.maxBrakePct = maxBrakePct; - } - - public boolean isInvertLeftBrakeDirection() { - return invertLeftBrakeDirection; - } - - public void setInvertLeftBrakeDirection(boolean invertLeftBrakeDirection) { - this.invertLeftBrakeDirection = invertLeftBrakeDirection; - } - - public boolean isInvertRightBrakeDirection() { - return invertRightBrakeDirection; - } - - public void setInvertRightBrakeDirection(boolean invertRightBrakeDirection) { - this.invertRightBrakeDirection = invertRightBrakeDirection; - } - - public double getBrakingStopZone() { - return brakingStopZone; - } - - public void setBrakingStopZone(double brakingStopZone) { - this.brakingStopZone = brakingStopZone; - } - - public Joystick getJoystick() { - return joystick; - } - - public void setJoystick(Joystick joystick) { - this.joystick = joystick; - } - - public int getLeftAxis() { - return leftAxis; - } - - public void setLeftAxis(int leftAxis) { - this.leftAxis = leftAxis; - } - - public boolean isInvertLeft() { - return invertLeft; - } - - public void setInvertLeft(boolean invertLeft) { - this.invertLeft = invertLeft; - } - - public int getRightAxis() { - return rightAxis; - } - - public void setRightAxis(int rightAxis) { - this.rightAxis = rightAxis; - } - - public boolean isInvertRight() { - return invertRight; - } - - public void setInvertRight(boolean invertRight) { - this.invertRight = invertRight; - } - - public int getForwardTrigger() { - return forwardTrigger; - } - - public void setForwardTrigger(int forwardTrigger) { - this.forwardTrigger = forwardTrigger; - } - - public int getBackTrigger() { - return backTrigger; - } - - public void setBackTrigger(int backTrigger) { - this.backTrigger = backTrigger; - } - - public double getDeadzone() { - return deadzone; - } - - public void setDeadzone(double deadzone) { - this.deadzone = deadzone; - } - - public double getBrakeOverrideThresh() { - return brakeOverrideThresh; - } - - public void setBrakeOverrideThresh(double brakeOverrideThresh) { - this.brakeOverrideThresh = brakeOverrideThresh; - } - - private void doPeakOutput(ChickenController controller, double setpoint, boolean invertBrake) { - boolean stopped = abs(controller.getSelectedSensorVelocity()) < abs(brakingStopZone * maxVel); - - if (!stopped && setpoint < brakeOverrideThresh) { - // process braking - boolean goingForward = - Utilities.invertIf(invertBrake, controller.getSelectedSensorVelocity()) > 0; - configPeakOutput.accept(controller, goingForward ? 1 : maxBrakePct, goingForward ? - -maxBrakePct : -1); - } else { - configPeakOutput.accept(controller, 1.0, -1.0); - } - } - - @FunctionalInterface - public interface TriConsumer { - - void accept(T t, U u, V v); - - default TriConsumer andThen( - TriConsumer after) { - Objects.requireNonNull(after); - return (a, b, c) -> { - accept(a, b, c); - after.accept(a, b, c); - }; - } - } -} diff --git a/src/main/java/org/team1540/rooster/drive/PidDriveConfiguration.java b/src/main/java/org/team1540/rooster/drive/PidDriveConfiguration.java deleted file mode 100644 index 491bffce..00000000 --- a/src/main/java/org/team1540/rooster/drive/PidDriveConfiguration.java +++ /dev/null @@ -1,27 +0,0 @@ -package org.team1540.rooster.drive; - -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.command.Subsystem; -import org.team1540.rooster.wrappers.ChickenController; - -class PidDriveConfiguration { - - Subsystem subsystem; - ChickenController left; - ChickenController right; - double maxVel; - JoystickScaling scaling; - double maxBrakePct; - boolean invertLeftBrakeDirection; - boolean invertRightBrakeDirection; - double brakingStopZone; - Joystick joystick; - int leftAxis; - boolean invertLeft; - int rightAxis; - boolean invertRight; - int forwardTrigger; - int backTrigger; - double deadzone; - double brakeOverrideThresh; -} diff --git a/src/main/java/org/team1540/rooster/drive/PidDriveFactory.java b/src/main/java/org/team1540/rooster/drive/PidDriveFactory.java deleted file mode 100644 index a08a4bfc..00000000 --- a/src/main/java/org/team1540/rooster/drive/PidDriveFactory.java +++ /dev/null @@ -1,336 +0,0 @@ -package org.team1540.rooster.drive; - -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.command.Subsystem; -import org.team1540.rooster.wrappers.ChickenController; - -/** - * Factory class for producing {@link PidDrive} instances. - * - * @see PidDrive - */ -public class PidDriveFactory { - - private Subsystem subsystem; - private ChickenController left; - private ChickenController right; - - private double maxVel; - private JoystickScaling scaling = new PowerJoystickScaling(2); - private double maxBrakePct = 0.1; - private boolean invertLeftBrakeDirection = false; - private boolean invertRightBrakeDirection = false; - private double brakingStopZone = 0.1; - - private Joystick joystick; - private int leftAxis; - private boolean invertLeft = false; - private int rightAxis; - private boolean invertRight = false; - private int forwardTrigger; - private int backTrigger; - private double deadzone = 0.1; - private double brakeOverrideThresh = 0.9; - - public Subsystem getSubsystem() { - return subsystem; - } - - /** - * Sets the subsystem used by this command for command creation. - */ - public PidDriveFactory setSubsystem(Subsystem subsystem) { - this.subsystem = subsystem; - return this; - } - - public ChickenController getLeft() { - return left; - } - - /** - * Sets the left motor controller. - * - * @param left The motor controller to set. - * @return An instance of this {@code PidDriveFactory} in a builder pattern. - */ - public PidDriveFactory setLeft(ChickenController left) { - this.left = left; - return this; - } - - public ChickenController getRight() { - return right; - } - - /** - * Sets the right motor controller. - * - * @param right The motor controller to set. - * @return An instance of this {@code PidDriveFactory} in a builder pattern. - */ - public PidDriveFactory setRight(ChickenController right) { - this.right = right; - return this; - } - - public double getMaxVel() { - return maxVel; - } - - /** - * Set the maximum velocity of the powered drive mechanism. - * - * @param maxVel The maximum velocity, in motor controller native units. - * @return An instance of this {@code PidDriveFactory} in a builder pattern. - */ - public PidDriveFactory setMaxVel(double maxVel) { - this.maxVel = maxVel; - return this; - } - - public JoystickScaling getScaling() { - return scaling; - } - - /** - * Set the scaling function to apply to joystick inputs. Default: a {@link PowerJoystickScaling} - * with a power of 2. - * - * @param scaling The scaling function. - * @return An instance of this {@code PidDriveFactory} in a builder pattern. - */ - public PidDriveFactory setScaling(JoystickScaling scaling) { - this.scaling = scaling; - return this; - } - - public double getMaxBrakePct() { - return maxBrakePct; - } - - /** - * Set the maximum allowable backdriving throttle. Defaults to 0.1. - * - * @param maxBrakePct The max throttle, in percent. - * @return An instance of this {@code PidDriveFactory} in a builder pattern. - */ - public PidDriveFactory setMaxBrakePct(double maxBrakePct) { - this.maxBrakePct = maxBrakePct; - return this; - } - - public boolean isInvertLeftBrakeDirection() { - return invertLeftBrakeDirection; - } - - /** - * Sets whether to invert the left-side braking algorithm forwards direction. Defaults to {@code - * false}. - * - * @param invertLeftBrakeDirection Whether or not to invert. - * @return An instance of this {@code PidDriveFactory} in a builder pattern. - */ - public PidDriveFactory setInvertLeftBrakeDirection(boolean invertLeftBrakeDirection) { - this.invertLeftBrakeDirection = invertLeftBrakeDirection; - return this; - } - - public boolean isInvertRightBrakeDirection() { - return invertRightBrakeDirection; - } - - /** - * Sets whether to invert the right-side braking algorithm forwards detection. Defaults to {@code - * false}. - * - * @param invertRightBrakeDirection Whether to invert. - * @return An instance of this {@code PidDriveFactory} in a builder pattern. - */ - public PidDriveFactory setInvertRightBrakeDirection(boolean invertRightBrakeDirection) { - this.invertRightBrakeDirection = invertRightBrakeDirection; - return this; - } - - public double getBrakingStopZone() { - return brakingStopZone; - } - - /** - * Sets the zone where the braking algorithm will consider the drive "stopped" for the purpose of - * determining braking. When the drive is stopped, the motors will not be output-limited. Defaults - * to 0.1. - * - * @param brakingStopZone The stop zone, in a fraction of the set max velocity. - * @return An instance of this {@code PidDriveFactory} in a builder pattern. - */ - public PidDriveFactory setBrakingStopZone(double brakingStopZone) { - this.brakingStopZone = brakingStopZone; - return this; - } - - public Joystick getJoystick() { - return joystick; - } - - /** - * Sets the joystick to use for command control. - * - * @param joystick The joystick. - * @return An instance of this {@code PidDriveFactory} in a builder pattern. - */ - public PidDriveFactory setJoystick(Joystick joystick) { - this.joystick = joystick; - return this; - } - - public int getLeftAxis() { - return leftAxis; - } - - /** - * Sets the joystick axis used for controlling the left-side motors. - * - * @param leftAxis The axis to use. - * @return An instance of this {@code PidDriveFactory} in a builder pattern. - */ - public PidDriveFactory setLeftAxis(int leftAxis) { - this.leftAxis = leftAxis; - return this; - } - - public boolean isInvertLeft() { - return invertLeft; - } - - /** - * Sets whether to invert the left joystick input. Defaults to {@code false}. - * - * @param invertLeft Whether or not to invert. - * @return An instance of this {@code PidDriveFactory} in a builder pattern. - */ - public PidDriveFactory setInvertLeft(boolean invertLeft) { - this.invertLeft = invertLeft; - return this; - } - - public int getRightAxis() { - return rightAxis; - } - - /** - * Sets the axis used for controlling the right-side motors. - * - * @param rightAxis The axis to use. - * @return An instance of this {@code PidDriveFactory} in a builder pattern. - */ - public PidDriveFactory setRightAxis(int rightAxis) { - this.rightAxis = rightAxis; - return this; - } - - public boolean isInvertRight() { - return invertRight; - } - - /** - * Sets whether to invert the right joystick input. Defaults to {@code false}. - * - * @param invertRight Whether or not to invert. - * @return An instance of this {@code PidDriveFactory} in a builder pattern. - */ - public PidDriveFactory setInvertRight(boolean invertRight) { - this.invertRight = invertRight; - return this; - } - - public int getForwardTrigger() { - return forwardTrigger; - } - - /** - * Sets the trigger axis used to apply straight forwards power. - * - * @param forwardTrigger The axis to set. - * @return An instance of this {@code PidDriveFactory} in a builder pattern. - */ - public PidDriveFactory setForwardTrigger(int forwardTrigger) { - this.forwardTrigger = forwardTrigger; - return this; - } - - public int getBackTrigger() { - return backTrigger; - } - - /** - * Sets the joystick axis used to apply straight backwards power. - * - * @param backTrigger The axis to set. - * @return An instance of this {@code PidDriveFactory} in a builder pattern. - */ - public PidDriveFactory setBackTrigger(int backTrigger) { - this.backTrigger = backTrigger; - return this; - } - - public double getDeadzone() { - return deadzone; - } - - /** - * Sets the joystick deadzone (joystick inputs within this distance from 0 will be ignored). - * Defaults to 0.1. - * - * @param deadzone The deadzone to set - * @return An instance of this {@code PidDriveFactory} in a builder pattern. - */ - public PidDriveFactory setDeadzone(double deadzone) { - this.deadzone = deadzone; - return this; - } - - public double getBrakeOverrideThresh() { - return brakeOverrideThresh; - } - - /** - * Sets the joystick threshold above which the robot will apply full reverse power to fight moving - * backwards. Defaults to 0.9. - * - * @param brakeOverrideThresh The threshold to set. - * @return An instance of this {@code PidDriveFactory} in a builder pattern. - */ - public PidDriveFactory setBrakeOverrideThresh(double brakeOverrideThresh) { - this.brakeOverrideThresh = brakeOverrideThresh; - return this; - } - - /** - * Creates a new {@code PidDrive} instance with previously-specified parameters. - * - * @return A new PidDrive. - */ - public PidDrive createPidDrive() { - PidDriveConfiguration config = new PidDriveConfiguration(); - config.subsystem = subsystem; - config.left = left; - config.right = right; - config.maxVel = maxVel; - config.scaling = scaling; - config.maxBrakePct = maxBrakePct; - config.invertLeftBrakeDirection = invertLeftBrakeDirection; - config.invertRightBrakeDirection = invertRightBrakeDirection; - config.brakingStopZone = brakingStopZone; - config.joystick = joystick; - config.leftAxis = leftAxis; - config.invertLeft = invertLeft; - config.rightAxis = rightAxis; - config.invertRight = invertRight; - config.forwardTrigger = forwardTrigger; - config.backTrigger = backTrigger; - config.deadzone = deadzone; - config.brakeOverrideThresh = brakeOverrideThresh; - return new PidDrive(config); - } -} diff --git a/src/main/java/org/team1540/rooster/drive/PowerJoystickScaling.java b/src/main/java/org/team1540/rooster/drive/PowerJoystickScaling.java deleted file mode 100644 index e66cfccd..00000000 --- a/src/main/java/org/team1540/rooster/drive/PowerJoystickScaling.java +++ /dev/null @@ -1,39 +0,0 @@ -package org.team1540.rooster.drive; - -/** - * Joystick scaling based on raising the joystick input to a set power. - */ -public class PowerJoystickScaling implements JoystickScaling { - - private double pow; - - /** - * Creates a new {@code PowerJoystickScaling} instance with the provided power. - * - * @param pow The power to raise the joystick input to during scaling. - */ - public PowerJoystickScaling(double pow) { - this.pow = pow; - } - - /** - * Scales the provided input using the equation {@code |input|}{@code pow}, where - * {@code pow} is the power set via constructor or using {@link #setPow(double) setPow()}. - * - * @param input The input to scale. - * @return The absolute value of the input, raised to a specified power, with the sign (positive - * or negative) of the original input. - */ - @Override - public double scale(double input) { - return Math.copySign(Math.pow(Math.abs(input), pow), input); - } - - public double getPow() { - return pow; - } - - public void setPow(double pow) { - this.pow = pow; - } -} diff --git a/src/main/java/org/team1540/rooster/drive/package-info.java b/src/main/java/org/team1540/rooster/drive/package-info.java index 3ac594b3..a8c92269 100644 --- a/src/main/java/org/team1540/rooster/drive/package-info.java +++ b/src/main/java/org/team1540/rooster/drive/package-info.java @@ -1,5 +1,4 @@ /** - * Contains various implementations of custom advanced drive code, as well as the {@linkplain - * org.team1540.rooster.drive.pipeline pipeline} system. + * Contains the {@linkplain org.team1540.rooster.drive.pipeline drive pipeline} system. */ package org.team1540.rooster.drive; diff --git a/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java b/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java index 5ae80848..eead97a6 100644 --- a/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java +++ b/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java @@ -3,8 +3,8 @@ import java.util.OptionalDouble; import java.util.function.DoubleSupplier; import org.jetbrains.annotations.NotNull; -import org.team1540.rooster.Utilities; import org.team1540.rooster.functional.Input; +import org.team1540.rooster.util.MathUtils; /** * Modified arcade drive joystick input. @@ -84,10 +84,10 @@ public TankDriveData get() { // scale the soft turn by the throttle, but don't scale the hard turn // add turn value to left and subtract from right double leftPowerRaw = throttle - + (soft * Utilities.invertIf(reverseBackwards && throttle < 0, Math.abs(throttle))) + + (soft * MathUtils.negateDoubleIf(reverseBackwards && throttle < 0, Math.abs(throttle))) + hard; double rightPowerRaw = throttle - - (soft * Utilities.invertIf(reverseBackwards && throttle < 0, Math.abs(throttle))) + - (soft * MathUtils.negateDoubleIf(reverseBackwards && throttle < 0, Math.abs(throttle))) - hard; // scale the powers, so if the total power for one side is greater than 1 we start reducing the diff --git a/src/main/java/org/team1540/rooster/drive/pipeline/SimpleJoystickInput.java b/src/main/java/org/team1540/rooster/drive/pipeline/SimpleJoystickInput.java index fc5f19d1..160a8a30 100644 --- a/src/main/java/org/team1540/rooster/drive/pipeline/SimpleJoystickInput.java +++ b/src/main/java/org/team1540/rooster/drive/pipeline/SimpleJoystickInput.java @@ -1,8 +1,9 @@ package org.team1540.rooster.drive.pipeline; import edu.wpi.first.wpilibj.Joystick; -import org.team1540.rooster.Utilities; import org.team1540.rooster.functional.Input; +import org.team1540.rooster.util.ControlUtils; +import org.team1540.rooster.util.MathUtils; /** * Simple tank-style input from a WPILib {@link Joystick}. The left and right joysticks are used to @@ -26,23 +27,23 @@ public class SimpleJoystickInput implements Input { @Override public TankDriveData get() { - double triggerValue; - if (fwdAxis != -1 && backAxis != -1) { - triggerValue = Utilities.processDeadzone(joystick.getRawAxis(fwdAxis), deadzone) - - Utilities.processDeadzone(joystick.getRawAxis(backAxis), deadzone); - } else { - triggerValue = 0; - } - double leftThrottle = Utilities.constrain( - Utilities.processDeadzone( - Utilities.invertIf(invertLeft, joystick.getRawAxis(leftAxis)), deadzone - ) + triggerValue, 1); - double rightThrottle = Utilities.constrain( - Utilities.processDeadzone( - Utilities.invertIf(invertRight, joystick.getRawAxis(rightAxis)), deadzone - ) + triggerValue, 1); + double triggerValue; + if (fwdAxis != -1 && backAxis != -1) { + triggerValue = ControlUtils.deadzone(joystick.getRawAxis(fwdAxis), deadzone) + - ControlUtils.deadzone(joystick.getRawAxis(backAxis), deadzone); + } else { + triggerValue = 0; + } + double leftThrottle = MathUtils.constrain( + ControlUtils.deadzone( + MathUtils.negateDoubleIf(invertLeft, joystick.getRawAxis(leftAxis)), deadzone + ) + triggerValue, 1); + double rightThrottle = MathUtils.constrain( + ControlUtils.deadzone( + MathUtils.negateDoubleIf(invertRight, joystick.getRawAxis(rightAxis)), deadzone + ) + triggerValue, 1); - return new TankDriveData().withAdditionalFeedForwards(leftThrottle, rightThrottle); + return new TankDriveData().withAdditionalFeedForwards(leftThrottle, rightThrottle); } /** @@ -98,8 +99,7 @@ public SimpleJoystickInput(Joystick joystick, int leftAxis, int rightAxis, int f * axis. * @param invertLeft Whether to invert the axis value of the left axis. * @param invertRight Whether to invert the axis value of the right axis. - * @param deadzone The deadzone for the axes (see {@link Utilities#processDeadzone(double, - * double)}). + * @param deadzone The deadzone for the axes (see {@link ControlUtils#deadzone(double, double)}). */ public SimpleJoystickInput(Joystick joystick, int leftAxis, int rightAxis, int fwdAxis, int backAxis, boolean invertLeft, boolean invertRight, double deadzone) { diff --git a/src/main/java/org/team1540/rooster/logging/DriverStationAppender.java b/src/main/java/org/team1540/rooster/logging/DriverStationAppender.java new file mode 100644 index 00000000..2a4721df --- /dev/null +++ b/src/main/java/org/team1540/rooster/logging/DriverStationAppender.java @@ -0,0 +1,37 @@ +package org.team1540.rooster.logging; + +import edu.wpi.first.wpilibj.DriverStation; +import org.apache.log4j.AppenderSkeleton; +import org.apache.log4j.Level; +import org.apache.log4j.spi.LoggingEvent; + +/** + * Translates Log4J log messages at Error or Warning level to driver station calls. Calls at level + * {@link Level#ERROR} or above will be reported using {@link DriverStation#reportError(String, + * boolean) DriverStation.reportError()}, while calls below {@link Level#ERROR} but at or above + * level {@link Level#WARN} will be reported using {@link DriverStation#reportWarning(String, + * boolean) DriverStation#reportWarning()} + */ +public class DriverStationAppender extends AppenderSkeleton { + + @Override + protected void append(LoggingEvent loggingEvent) { + if (loggingEvent.getLevel().isGreaterOrEqual(Level.WARN)) { + if (loggingEvent.getLevel().isGreaterOrEqual(Level.ERROR)) { + DriverStation.reportError(layout.format(loggingEvent), false); + } else { + DriverStation.reportWarning(layout.format(loggingEvent), false); + } + } + } + + @Override + public void close() { + + } + + @Override + public boolean requiresLayout() { + return true; + } +} diff --git a/src/main/java/org/team1540/rooster/motionprofiling/MotionProfileUtils.java b/src/main/java/org/team1540/rooster/motionprofiling/MotionProfileUtils.java deleted file mode 100644 index fd9e0563..00000000 --- a/src/main/java/org/team1540/rooster/motionprofiling/MotionProfileUtils.java +++ /dev/null @@ -1,30 +0,0 @@ -package org.team1540.rooster.motionprofiling; - -import jaci.pathfinder.Trajectory; -import java.util.Arrays; -import org.jetbrains.annotations.Contract; -import org.jetbrains.annotations.NotNull; -import org.team1540.rooster.motionprofiling.MotionProfile.Point; - -public class MotionProfileUtils { - - private MotionProfileUtils() { - } - - /** - * Creates a ROOSTER {@link MotionProfile} from a Pathfinder {@link Trajectory}. - * - * @param trajectory The {@link Trajectory} to convert. - * @return A {@link MotionProfile} containing the same points. Profile points are copied over, so - * subsequent changes to the {@link Trajectory} will not affect the produced {@link - * MotionProfile}. - */ - @Contract("_ -> new") - @NotNull - public static MotionProfile createProfile(@NotNull Trajectory trajectory) { - return new MotionProfile(Arrays.stream(trajectory.segments).map( - s -> new Point(s.dt, s.x, s.y, s.position, s.velocity, s.acceleration, s.jerk, s.heading)) - .toArray(Point[]::new)); - } - -} diff --git a/src/main/java/org/team1540/rooster/motionprofiling/MotionProfilingProperties.java b/src/main/java/org/team1540/rooster/motionprofiling/MotionProfilingProperties.java deleted file mode 100644 index 6238b7d9..00000000 --- a/src/main/java/org/team1540/rooster/motionprofiling/MotionProfilingProperties.java +++ /dev/null @@ -1,121 +0,0 @@ -package org.team1540.rooster.motionprofiling; - -import jaci.pathfinder.Trajectory; -import java.util.function.DoubleConsumer; -import java.util.function.DoubleSupplier; - -/** - * @deprecated Replaced by the {@link org.team1540.rooster.drive.pipeline drive pipeline}-based - * system. - */ -@Deprecated -public class MotionProfilingProperties { - - private double encoderTicksPerUnit = 1023 * 0.1 * Math.PI; - - private double secondsFromNeutralToFull = 0; - - private DoubleSupplier getEncoderVelocityFunction; - private DoubleConsumer setMotorVelocityFunction; - - // Okay so this is actually pretty much unused, but here because we kinda might plan on using it - // in the future and it's useful to have - private DoubleSupplier getEncoderPositionFunction; - - private Trajectory trajectory; - - private double currentTime = 0; - - public MotionProfilingProperties(DoubleSupplier getEncoderVelocityFunction, - DoubleConsumer setMotorVelocityFunction, DoubleSupplier getEncoderPositionFunction, - Trajectory trajectory) { - this.getEncoderVelocityFunction = getEncoderVelocityFunction; - this.setMotorVelocityFunction = setMotorVelocityFunction; - this.getEncoderPositionFunction = getEncoderPositionFunction; - this.trajectory = trajectory; - } - - public MotionProfilingProperties(double encoderTicksPerUnit, double secondsFromNeutralToFull, - DoubleSupplier getEncoderVelocityFunction, DoubleConsumer setMotorVelocityFunction, - DoubleSupplier getEncoderPositionFunction, Trajectory trajectory) { - this.encoderTicksPerUnit = encoderTicksPerUnit; - this.secondsFromNeutralToFull = secondsFromNeutralToFull; - this.getEncoderVelocityFunction = getEncoderVelocityFunction; - this.setMotorVelocityFunction = setMotorVelocityFunction; - this.getEncoderPositionFunction = getEncoderPositionFunction; - this.trajectory = trajectory; - } - - /** - * Get the number of encoder ticks per revolution of the wheel. This is roughly equal to the - * number of encoder ticks per rev * the wheel diameter * pi. - * - * @return The number of encoder ticks in native units. - */ - public double getEncoderTicksPerUnit() { - return encoderTicksPerUnit; - } - - /** - * Set the number of encoder ticks per unit distance travelled by the wheel. You can calculate - * this by doing the number of encoder ticks per rev * the wheel diameter * pi. - * - * @param encoderTicksPerUnit The number of encoder ticks in native units. - */ - public void setEncoderTicksPerUnit(double encoderTicksPerUnit) { - this.encoderTicksPerUnit = encoderTicksPerUnit; - } - - /** - * Get the ratio of encoder ticks to distance travelled by the wheel. Multiply this number by a - * number of encoder ticks to get the distance travelled by the wheel. - * - * @return The ratio in encoder ticks / unit used in the Trajectory. - */ - public double getEncoderTickRatio() { - return (1 / encoderTicksPerUnit); - } - - public Trajectory getTrajectory() { - return trajectory; - } - - public double getSecondsFromNeutralToFull() { - return secondsFromNeutralToFull; - } - - public void setSecondsFromNeutralToFull(double secondsFromNeutralToFull) { - this.secondsFromNeutralToFull = secondsFromNeutralToFull; - } - - public DoubleSupplier getGetEncoderVelocityFunction() { - return getEncoderVelocityFunction; - } - - /** - * Sets the function that sets the encoder velocity. This function will be passed the velocity in - * native units / decisecond. - * - * @param getEncoderVelocityFunction DoubleSupplier that sets the encoder velocity. - */ - public void setGetEncoderVelocityFunction(DoubleSupplier getEncoderVelocityFunction) { - this.getEncoderVelocityFunction = getEncoderVelocityFunction; - } - - public DoubleConsumer getSetMotorVelocityFunction() { - return setMotorVelocityFunction; - } - - public void setSetMotorVelocityFunction(DoubleConsumer setMotorVelocityFunction) { - this.setMotorVelocityFunction = setMotorVelocityFunction; - } - - public DoubleSupplier getGetEncoderPositionFunction() { - return getEncoderPositionFunction; - } - - public void setGetEncoderPositionFunction(DoubleSupplier getEncoderPositionFunction) { - this.getEncoderPositionFunction = getEncoderPositionFunction; - } - -} diff --git a/src/main/java/org/team1540/rooster/motionprofiling/ProfileContainer.java b/src/main/java/org/team1540/rooster/motionprofiling/ProfileContainer.java deleted file mode 100644 index b0359599..00000000 --- a/src/main/java/org/team1540/rooster/motionprofiling/ProfileContainer.java +++ /dev/null @@ -1,188 +0,0 @@ -package org.team1540.rooster.motionprofiling; - -import edu.wpi.first.wpilibj.DriverStation; -import jaci.pathfinder.Pathfinder; -import java.io.File; -import java.util.Arrays; -import java.util.HashMap; -import java.util.HashSet; -import java.util.Map; -import java.util.Set; -import org.jetbrains.annotations.NotNull; -import org.jetbrains.annotations.Nullable; - -/** - * Class to preload motion profiles from a specified folder on disk. The preloaded motion profiles - * are stored in RAM so as to be quickly accessible. - * - *

The folder provided should contain profile CSV files (formatted so as to be read by {@link - * Pathfinder#readFromCSV(File)}) where each profile named {@code name} is stored in two files, - * {@code name}_left.csv for the left-side profile and {@code name}_right.csv for the right-side - * profile. An example folder structure would be as follows (where the profiles/ directory is - * provided to the constructor): - * - * 

- * profiles/
- *   foo_left.csv
- *   foo_right.csv
- *   bar_left.csv
- *   bar_right.csv
- *
- * - * This would cause the {@code ProfileContainer} to load two profiles named {@code foo} and {@code - * bar}. - * - *

This class is immutable after instantiation. - */ -public class ProfileContainer { - - @NotNull - private Map profiles; - - /** - * Creates a new {@code ProfileContainer} that searches the provided directory using a left suffix - * of "{@code _left.csv}" and a right suffix of "{@code _right.csv}" - * - * This constructor also searches the provided directory for profiles and loads all the profiles - * into RAM. For this reason, initialization may take some time (especially for large amounts of - * profiles). - * - * @param profileDirectory The directory containing the profiles. See the {@linkplain - * ProfileContainer class documentation} for a description of the folder structure. - * @throws RuntimeException If an I/O error occurs during profile loading. - */ - public ProfileContainer(@NotNull File profileDirectory) { - this(profileDirectory, "_left.csv", "_right.csv"); - } - - /** - * Creates a new {@code ProfileContainer}. This constructor also searches the provided directory - * for profiles and loads all the profiles into RAM. For this reason, initialization may take some - * time (especially for large amounts of profiles). - * - * @param profileDirectory The directory containing the profiles. See the {@linkplain - * ProfileContainer class documentation} for a description of the folder structure. - * @param leftSuffix The suffix to use to identify left-side profile files. - * @param rightSuffix The suffix to use to identify right-side profile files. - * @throws RuntimeException If an I/O error occurs during profile loading. - */ - public ProfileContainer(@NotNull File profileDirectory, @NotNull String leftSuffix, - @NotNull String rightSuffix) { - if (!profileDirectory.isDirectory()) { - throw new IllegalArgumentException("Not a directory"); - } - - File[] lFiles = profileDirectory.listFiles((file) -> file.getName().endsWith(leftSuffix)); - File[] rFiles = profileDirectory.listFiles((file) -> file.getName().endsWith(rightSuffix)); - - if (lFiles == null || rFiles == null) { - // according to listFiles() docs, it will only return null if the file isn't a directory - // (which we've already checked) or if an IO error occurs. Thus, if lFiles or rFiles is - // null we know an IO error happened. Not throwing an IOException because checked exceptions - // are bad, in constructors even more so. - throw new RuntimeException("IO error occurred while reading files"); - } - - Set profileNames = new HashSet<>(); - - for (File f : lFiles) { - profileNames.add(f.getName().substring(0, f.getName().length() - leftSuffix.length())); - } - - for (File f : rFiles) { - profileNames.add(f.getName().substring(0, f.getName().length() - rightSuffix.length())); - } - - // initialize the map once we know the number of profiles so it doesn't expand. - // Why? p e r f o r m a n c e - - profiles = new HashMap<>(profileNames.size()); - - for (String name : profileNames) { - System.out.println("Loading profile " + name); - File leftFile = Arrays.stream(lFiles) - .filter(file -> file.getName().equals(name + leftSuffix)) - .findFirst() - .orElseGet(() -> { - DriverStation - .reportWarning("Left-side file for profile " + name + " does not exist", false); - return null; - }); - File rightFile = Arrays.stream(rFiles) - .filter(file -> file.getName().equals(name + rightSuffix)) - .findFirst() - .orElseGet(() -> { - DriverStation - .reportWarning("Right-side file for profile " + name + " does not exist", false); - return null; - }); - - if (leftFile != null && rightFile != null) { - MotionProfile left = MotionProfileUtils.createProfile(Pathfinder.readFromCSV(leftFile)); - MotionProfile right = MotionProfileUtils.createProfile(Pathfinder.readFromCSV(rightFile)); - - profiles.put(name, new DriveProfile(left, right)); - } - } - } - - /** - * Returns the motion profile set with the specified name, or {@code null} if the profile does not - * exist. - * - * @param name The name of the profile. - * @return A {@link DriveProfile} containing the left and right profiles, or {@code null} if the - * profile does not exist. - */ - @Nullable - public DriveProfile get(String name) { - return profiles.get(name); - } - - /** - * Get the {@link Set} of profile names in the {@code ProfileContainer}. - * - * @return A {@link Set} containing the names of each profile. - */ - @NotNull - public Set getProfileNames() { - return profiles.keySet(); - } - - /** - * Returns whether or not the {@code ProfileContainer} contains a specific profile. - * - * @param name The name of the profile to check. - * @return {@code true} if the profile exists (i.e. if {@link #get(String) get(name)} would not - * return {@code null}), {@code false} otherwise. - */ - public boolean hasProfile(String name) { - return profiles.containsKey(name); - } - - /** - * Data class for holding left and right trajectories in one object. - */ - public static class DriveProfile { - - @NotNull - private final MotionProfile left; - @NotNull - private final MotionProfile right; - - private DriveProfile(@NotNull MotionProfile left, @NotNull MotionProfile right) { - this.left = left; - this.right = right; - } - - @NotNull - public MotionProfile getLeft() { - return left; - } - - @NotNull - public MotionProfile getRight() { - return right; - } - } -} diff --git a/src/main/java/org/team1540/rooster/motionprofiling/RunMotionProfiles.java b/src/main/java/org/team1540/rooster/motionprofiling/RunMotionProfiles.java deleted file mode 100644 index 3bd922db..00000000 --- a/src/main/java/org/team1540/rooster/motionprofiling/RunMotionProfiles.java +++ /dev/null @@ -1,125 +0,0 @@ -package org.team1540.rooster.motionprofiling; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.command.Command; -import jaci.pathfinder.Trajectory; -import java.util.Arrays; -import java.util.HashSet; -import java.util.Set; - -/** - * Executes a set of motion profiles (with respective properties.) - * - * @deprecated Replaced by the {@link org.team1540.rooster.drive.pipeline drive pipeline}-based - * system. - */ -@Deprecated -public class RunMotionProfiles extends Command { - - private int slotId = 0; - private Set motionProfiles; - private Timer timer = new Timer(); - private double lastTime; - private boolean isFinished = false; - - public RunMotionProfiles(MotionProfilingProperties... properties) { - realConstructor(new HashSet<>(Arrays.asList(properties))); - } - - public RunMotionProfiles(Set motionProfiles) { - realConstructor(motionProfiles); - } - - private void realConstructor(Set motionProfiles) { - this.motionProfiles = motionProfiles; - } - - public int getSlotId() { - return slotId; - } - - public void setSlotId(int slotId) { - this.slotId = slotId; - } - - public Set getMotionProfiles() { - return motionProfiles; - } - - public void setMotionProfiles( - Set motionProfiles) { - this.motionProfiles = motionProfiles; - } - - @Override - protected void initialize() { - timer.start(); - lastTime = timer.get(); - isFinished = false; - } - - @Override - protected void execute() { - for (MotionProfilingProperties currentProperty : motionProfiles) { - // Each controller's setpoint is calculated at a slightly different time, but this doesn't - // matter, since the motion profile is "continuous." - double velocity = getVelocitySetpoint(currentProperty, timer.get(), lastTime); - currentProperty.getSetMotorVelocityFunction().accept(velocity); - } - - lastTime = timer.get(); - } - - private double getVelocitySetpoint(MotionProfilingProperties currentProperty, double currentTime, - double lastTime) { - - Trajectory thisTrajectory = currentProperty.getTrajectory(); - double dt = thisTrajectory.segments[0].dt; - double encoderMultiplier = currentProperty.getEncoderTickRatio(); - double quadraturePosition = currentProperty.getGetEncoderPositionFunction().getAsDouble(); - - // Start from the current time and find the closest point. - int startIndex = Math.toIntExact(Math.round(currentTime / dt)); - - int length = thisTrajectory.segments.length; - int index = startIndex; - if (startIndex >= length - 1) { - index = length - 1; - isFinished = true; - } - return thisTrajectory.segments[index].velocity / encoderMultiplier * 0.1; - } - - /** - * Sets the velocity to 0 for all properties and sets isFinished to true. - */ - @Override - protected void interrupted() { - for (MotionProfilingProperties currentProperty : motionProfiles) { - currentProperty.getSetMotorVelocityFunction().accept(0); - } - isFinished = true; - } - - /** - * Calculate if a number is between (inclusive) two other numbers - * - * @param x The target - * @param x1 Lower bound - * @param x2 Upper bound - * @return if x is between x1 and x2 - */ - private boolean isBetween(double x, double x1, double x2) { - return (x >= x1 && x <= x2 || x >= x2 && x <= x1); - } - - private double linearInterpolation(double x, double lowX, double lowY, double highX, - double highY) { - return (x - lowX) * (highY - lowY) / (highX - lowX) + lowY; - } - - @Override - public boolean isFinished() { - return isFinished; - } -} diff --git a/src/main/java/org/team1540/rooster/motionprofiling/package-info.java b/src/main/java/org/team1540/rooster/motionprofiling/package-info.java index cc7256cf..49d2ef77 100644 --- a/src/main/java/org/team1540/rooster/motionprofiling/package-info.java +++ b/src/main/java/org/team1540/rooster/motionprofiling/package-info.java @@ -1,8 +1,6 @@ /** * Utilities for motion profiles. * - * This class contains common data classes to encapsulate motion profiles as well as a utility - * ({@link org.team1540.rooster.motionprofiling.ProfileContainer}) to load them from disk. It also - * contains legacy profile execution code, kept for backwards compatibility. + * This class contains a common class to encapsulate motion profiles. */ package org.team1540.rooster.motionprofiling; diff --git a/src/main/java/org/team1540/rooster/power/PowerManageable.java b/src/main/java/org/team1540/rooster/power/PowerManageable.java deleted file mode 100644 index 110970c5..00000000 --- a/src/main/java/org/team1540/rooster/power/PowerManageable.java +++ /dev/null @@ -1,78 +0,0 @@ -package org.team1540.rooster.power; - -import edu.wpi.first.wpilibj.Sendable; -import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; -import java.util.Optional; -import org.jetbrains.annotations.NotNull; - -@SuppressWarnings("unused") -public interface PowerManageable extends Comparable, Sendable { - - /** - * Get the priority of this PowerManageable. Used for power management. - * - * @return The priority of this PowerManageable. - */ - double getPriority(); - - /** - * Sets the priority of this PowerManageable. Used for power management. - * - * @param priority The priority to set. - */ - void setPriority(double priority); - - /** - * Gets the current percent output that the motors are being capped at. - * - * @return The current percent output from 0 to 1 (not enforced!) - */ - double getPercentOutputLimit(); - - /** - * Set the percent of the current power draw this PowerManageable can draw, - * e.g. if you were drawing .5 and set this to .5, you'll draw .25 - * @param limit The percent of the current power draw to draw, between 0 and 1 inclusive. - * @return Any excess percentOutput (i.e. any excess above 1.0, as that is the peak output of - * the motor.) - */ - double setRelativePercentOutputLimit(double limit); - - /** - * Stop limiting the power. - */ - void stopLimitingPower(); - - /** - * Gets the class responsible for grabbing the power telemetry, including power, current, and - * voltage. - * - * @return The PowerTelemetry contained in an optional, empty if it does not exist. - */ - Optional getPowerTelemetry(); - - /** - * Compare two PowerManageables by priority. - * - * @param o PowerManageables to compare to. - * @return (int) (getPriority() - o.getPriority()) - */ - @Override - default int compareTo(@NotNull PowerManageable o) { - return (int) (getPriority() - o.getPriority()); - } - - @Override - default void initSendable(SendableBuilder builder) { - sendablePowerInfo(builder); - } - - default void sendablePowerInfo(SendableBuilder builder) { - builder.setSmartDashboardType("PowerManageable"); - builder.addDoubleProperty("priority", this::getPriority, this::setPriority); - builder.addDoubleProperty("percentOutputLimit", this::getPercentOutputLimit, - this::setRelativePercentOutputLimit - ); - } - -} diff --git a/src/main/java/org/team1540/rooster/power/PowerManager.java b/src/main/java/org/team1540/rooster/power/PowerManager.java deleted file mode 100644 index 255ac12b..00000000 --- a/src/main/java/org/team1540/rooster/power/PowerManager.java +++ /dev/null @@ -1,529 +0,0 @@ -package org.team1540.rooster.power; - -import edu.wpi.first.wpilibj.PowerDistributionPanel; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.Sendable; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; -import java.util.Collections; -import java.util.HashMap; -import java.util.Map; -import java.util.Optional; -import java.util.concurrent.ConcurrentHashMap; -import java.util.function.BiFunction; -import java.util.function.DoubleSupplier; -import org.jetbrains.annotations.NotNull; -import org.jetbrains.annotations.Nullable; - -/* -A word on language: Management is if this is running, scaling is if the power is actually being set -to be something different. - */ - -// Reminder that everything will need to be thread safe - -/** - * A class for power managing PowerManageables (both with and without telemetry.) Set a target - * voltage to stay above, register PowerManageables, and you're good to go! - */ -@SuppressWarnings("unused") -public class PowerManager extends Thread implements Sendable { - - @NotNull - private static String name = "PowerManager"; - - // Singleton - private static PowerManager theManager = new PowerManager(); - - static { - theManager.start(); - } - - private final Timer voltageTimer = new Timer(); - // Store the currently running PowerManageables - // For the love of everything, so there are no race conditions, do not access this except though - // synchronized blocks - private final Map powerManageables = new - ConcurrentHashMap<>(); - - @NotNull - private DoubleSupplier getTotalCurrent = (new PowerDistributionPanel())::getTotalCurrent; - private int updateDelay = 10; - /** - * Default to be a little higher than brownouts. - */ - private double voltageDipLow = 7.5; - private double voltageMargin = 0.5; - private double voltageDipLength = 0.25; - private double voltageTarget = 8.0; - private boolean running = true; - @NotNull - private BiFunction priorityScalingFunction = this::scaleExponential; - - private double priorityUnscaledTotal, priorityScaledTotal, priorityScaledNoTelemetryTotal, - currentUnscaledTotal, currentScaledTotal, highestPriority; - private int telemetryCount, noTelemetryCount; - - /** - * Gets the PowerManager. - * - * @return The singleton PowerManager instance. - */ - public static PowerManager getInstance() { - return theManager; - } - - @Override - public void run() { - while (true) { - // No whiles in here as that'd stop the last block from executing - if (running) { - if (isVoltageDipping()) { - if (voltageTimer.get() <= 0) { - // Calling the timer when it's already started seems to reset it. - voltageTimer.start(); - } - } else { - voltageTimer.stop(); - voltageTimer.reset(); - stopScaling(); - } - - if (hasTimePassedVoltage()) { - scalePower(); - } - } - - try { - sleep(updateDelay); - } catch (InterruptedException e) { - // end the thread - return; - } - } - } - - /** - * Returns d if it is finite, else returns the safeValue. - * @param d A double of any sort - * @param safeValue A safe value to return. - * @return A finite double. - */ - @SuppressWarnings("SameParameterValue") - private static double finiteMath(double d, double safeValue) { - return Double.isFinite(d) ? d : safeValue; - } - - /** - * Separate method to block PowerManageable registration/deregistration while stopping scaling. - */ - private synchronized void stopScaling() { - powerManageables.keySet().forEach(PowerManageable::stopLimitingPower); - } - - /** - * Determines if the voltage is currently dipping. If power limiting is not engaged, - * returns {@link RobotController#getBatteryVoltage()} < voltageDipLow || - * {@link RobotController#isBrownedOut()}; - * If power limiting is engaged, returns {@link PowerDistributionPanel#getVoltage()} < - * voltageDipLow + voltageMargin || {@link RobotController#isBrownedOut()}; - * - * @return Boolean representing if the voltage is dipping. - */ - public boolean isVoltageDipping() { - if (!hasTimePassedVoltage()) { - return RobotController.getBatteryVoltage() < voltageDipLow || RobotController.isBrownedOut(); - } else { - return RobotController.getBatteryVoltage() < voltageDipLow + voltageMargin || RobotController - .isBrownedOut(); - } - } - - private boolean hasTimePassedVoltage() { - return (voltageTimer.get() > voltageDipLength); - } - - /** - * Determine if power limiting has kicked in. - * - * @return True if power limiting has kicked in, false otherwise - */ - public boolean isLimiting() { - return hasTimePassedVoltage() && isVoltageDipping(); - } - - /** - * Scale the priority of a given {@link PowerManageable} using an inverse natural - * exponential. \(\frac{h}{e^{\left(h-x\right)}}\) where h is the - * highest priority and x is the priority. - * - * @param highestPriority The priority of the highest priority {@link PowerManageable} - * currently running. - * @param priority The priority of this {@link PowerManageable}. - * @return The scale factor for this {@link PowerManageable}. - */ - private double scaleExponential(double highestPriority, double priority) { - return highestPriority / (Math.exp(highestPriority - priority)); - } - - /** - * Registers the {@link PowerManageable} as being used. Blocks power scaling. - * - * @param toRegister The {@link PowerManageable} to register. - * @return true if the PowerManager did not already contain the specified element - */ - public boolean registerPowerManageable(@NotNull PowerManageable toRegister) { - return (powerManageables.put(toRegister, new CachedPowerProperties(toRegister)) == null); - } - - /** - * Registers a group of {@link PowerManageable}s. Calls registerPowerManager(). - * - * @param toRegister The {@link PowerManageable}s to register. - * @return A map of PowerManageables with the key true if the PowerManager did not already contain - * the specified element - */ - public synchronized Map registerPowerManageables( - PowerManageable... toRegister) { - HashMap success = new HashMap<>(); - for (PowerManageable register : toRegister) { - success.put(register, registerPowerManageable(register)); - } - return success; - } - - /** - * Unregisters the {@link PowerManageable} as being used. Blocks power scaling. - * - * @param toUnregister The {@link PowerManageable} to unregister. - * @return true if the PowerManager contained the specified element - */ - public boolean unregisterPowerManageable(PowerManageable toUnregister) { - return powerManageables.remove(toUnregister) != null; - } - - /** - * Unregisters a group of {@link PowerManageable}s. Calls - * {@link PowerManager#unregisterPowerManageable(PowerManageable)}}. - * - * @param toUnregister The {@link PowerManageable}s to unregister. - * @return A map of PowerManageables with the key true if the PowerManager contained the specified - * element - */ - public synchronized Map unregisterPowerManageables( - PowerManageable... toUnregister) { - HashMap success = new HashMap<>(); - for (PowerManageable unregister : toUnregister) { - success.put(unregister, unregisterPowerManageable(unregister)); - } - return success; - } - - public int getUpdateDelay() { - return updateDelay; - } - - /** - * Sets the time between power management cycles. Defaults to 5ms. - * - * @param updateDelay The time between power management cycles, in milliseconds. - */ - public void setUpdateDelay(int updateDelay) { - this.updateDelay = updateDelay; - } - - public boolean isRunning() { - return running; - } - - /** - * Sets the state of the power manager. Set to {@code true} to enable power management, set to - * {@code false} to disable management. - * - * @param running The state of the power manager. - */ - public void setRunning(boolean running) { - this.running = running; - } - - /** - * Gets the highest current time on any of the internal timers representing time from the most - * recent spike or dip. - * - * @return Double representing time. - */ - public double getPowerTime() { - return voltageTimer.get(); - } - - /** - * Gets the required voltage value for the robot to be considered dipping. Defaults to 7.2V. - * - * @return voltageDipLow The minimum dip value, in volts. - */ - public double getVoltageDipLow() { - return voltageDipLow; - } - - /** - * Sets the required voltage value for the robot to be considered dipping. Defaults to 7.2V. - * - * @param voltageDipLow The minimum dip value, in volts. - */ - public void setVoltageDipLow(double voltageDipLow) { - this.voltageDipLow = voltageDipLow; - } - - /** - * Gets how long the voltage must dip for before doing anything. Defaults to 0.25 seconds, must be - * {@literal >=}0. - * - * @return voltageDipLength The minimum actionable spike length, in seconds. - */ - public double getVoltageDipLength() { - return voltageDipLength; - } - - /** - * Sets how long the voltage must dip for before doing anything. Defaults to 0.25 seconds, must - * be {@literal >=}0. - * - * @param voltageDipLength The minimum actionable spike length, in seconds. - */ - public void setVoltageDipLength(double voltageDipLength) { - if (voltageDipLength < 0) { - throw new IllegalArgumentException("voltageDipLength must be >=0, got " + voltageDipLength); - } - this.voltageDipLength = voltageDipLength; - } - - /** - * Gets the voltageMargin within which, if power limiting has engaged, power management will - * remain engaged. Defaults to 0.5V. - * - * @return voltageMargin in volts. - */ - public double getVoltageMargin() { - return voltageMargin; - } - - /** - * Sets the voltageMargin within which, if power limiting has engaged, power management will - * remain engaged. Defaults to 0.5V, must be {@literal >=}0. - * - * @param voltageMargin in volts. - */ - public void setVoltageMargin(double voltageMargin) { - if (voltageMargin < 0) { - throw new IllegalArgumentException("voltageMargin must be >=0, got " + voltageMargin); - } - this.voltageMargin = voltageMargin; - } - - /** - * Gets the voltageTarget. Defaults to 8.0V. - * - * @return voltageTarget in volts. - */ - public double getVoltageTarget() { - return voltageTarget; - } - - /** - * Sets the voltageTarget. Defaults to 8.0V, must be {@literal >=}0. - */ - public void setVoltageTarget(double voltageTarget) { - if (voltageTarget < 0) { - throw new IllegalArgumentException("voltageTarget must be >=0, got " + voltageTarget); - } - this.voltageTarget = voltageTarget; - } - - @Override - public String getSubsystem() { - return name; - } - - @Override - public void setSubsystem(String subsystem) { - name = subsystem; - } - - @NotNull - public BiFunction getPriorityScalingFunction() { - return priorityScalingFunction; - } - - public void setPriorityScalingFunction( - @NotNull BiFunction priorityScalingFunction) { - this.priorityScalingFunction = priorityScalingFunction; - } - - /** - * Separate method to block {@link PowerManageable} registration/unregistration while actually - * scaling the power. - */ - private synchronized void scalePower() { - // If the PowerManageable has PowerTelemetry, we'll scale it using the arbitrary function. - // Otherwise, it'll be scaled using what power remains. - - highestPriority = Collections.max(powerManageables.keySet()).getPriority(); - // The amount of our current output we need to be at - final double percentToTarget = RobotController.getBatteryVoltage() / voltageTarget; - final double totalCurrentDraw = getTotalCurrent.getAsDouble(); - final double currentNeedToDecrease = (1 - percentToTarget) * totalCurrentDraw; - - // Reset the totals - priorityUnscaledTotal = 0; - priorityScaledTotal = 0; - priorityScaledNoTelemetryTotal = 0; - currentUnscaledTotal = 0; - currentScaledTotal = 0; - telemetryCount = 0; - noTelemetryCount = 0; - - powerManageables.values().forEach(CachedPowerProperties::refreshTelemetry); - - // Decide the split for how much we'll use the fancy scaling on and how much we'll use the - // simple flat scaling on. - final double percentSimpleScaling = priorityScaledNoTelemetryTotal / priorityScaledTotal; - final double percentFancyScaling = 1 - percentSimpleScaling; - - // IF THERE IS TELEMETRY - // Cache the percent we really want to target times the conversion factor between scaled - // current and unscaled current. - // Then, multiply that times the scaled current, add in any overflow, and then calculate the - // percent that is by dividing by the unscaled current draw. - - // IF THERE IS NOT - // Cache the percent we really want to target times and just do a flat scale since we don't - // know how to break it up to hit the total. - - final double cachedMathHasTelemetry = percentFancyScaling * percentToTarget * - (currentUnscaledTotal / currentScaledTotal); - final double cachedMathNoTelemetry = percentSimpleScaling * percentToTarget; - - // Overflow is to ensure that if there's excess power to go around, it gets spread around - // instead of just getting thrown away - double currentOverflow = 0; - for (CachedPowerProperties currentProperties : powerManageables.values()) { - // Includes checking against divide by zero. - double percentToDecreaseTo = - finiteMath(currentProperties.getCurrentUnscaled().isPresent() ? - (cachedMathHasTelemetry * currentProperties.getCurrentScaled().get() + - currentOverflow) / currentProperties.getCurrentUnscaled().get() - : cachedMathNoTelemetry, 1); - // Set the percentToDecreaseTo. This will return any overflow, which we can multiply by the - // unscaled current to get an actual value in amps and store away for later. - currentOverflow = - currentProperties.manageable.setRelativePercentOutputLimit(percentToDecreaseTo) * - currentProperties.getCurrentUnscaled().get(); - } - } - - @NotNull - public DoubleSupplier getGetTotalCurrent() { - return getTotalCurrent; - } - - public void setGetTotalCurrent(@NotNull DoubleSupplier getTotalCurrent) { - this.getTotalCurrent = getTotalCurrent; - } - - - private class CachedPowerProperties { - - @SuppressWarnings("NullableProblems") - @NotNull - private Double priorityUnscaled; - @SuppressWarnings("NullableProblems") - @NotNull - private Double priorityScaled; - @Nullable - private Double currentUnscaled; - @Nullable - private Double currentScaled; - @NotNull - private PowerManageable manageable; - - public CachedPowerProperties(@NotNull PowerManageable manageable) { - this.manageable = manageable; - refreshTelemetry(); - } - - public void refreshTelemetry() { - this.priorityUnscaled = manageable.getPriority(); - this.priorityScaled = priorityScalingFunction.apply(highestPriority, priorityUnscaled); - priorityUnscaledTotal += priorityUnscaled; - priorityScaledTotal += priorityScaled; - if (manageable.getPowerTelemetry().isPresent()) { - this.currentUnscaled = manageable.getPowerTelemetry().get().getCurrent(); - this.currentScaled = this.priorityScaled * currentUnscaled; - currentUnscaledTotal += currentUnscaled; - currentScaledTotal += currentScaled; - telemetryCount++; - } else { - this.currentUnscaled = null; - this.currentScaled = null; - priorityScaledNoTelemetryTotal += priorityScaled; - noTelemetryCount++; - } - } - - @NotNull - private Double getPriorityUnscaled() { - return priorityUnscaled; - } - - @NotNull - private Double getPriorityScaled() { - return priorityScaled; - } - - @NotNull - private Optional getCurrentUnscaled() { - return Optional.ofNullable(currentUnscaled); - } - - @NotNull - private Optional getCurrentScaled() { - return Optional.ofNullable(currentScaled); - } - } - - /** - * In case the PDP is being funky, this provides a way to get the total power from the - * controllers instead. - */ - public class GetPowerFromControllersDoubleSupplier implements DoubleSupplier { - - @Override - public synchronized double getAsDouble() { - double totalCurrent = 0; - for (PowerManageable currentManageable : powerManageables.keySet()) { - if (currentManageable.getPowerTelemetry().isPresent()) { - totalCurrent += currentManageable.getPowerTelemetry().get().getCurrent(); - } - } - return totalCurrent; - } - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("PowerManager"); - builder.addBooleanProperty("isVoltageDipping", this::isVoltageDipping, null); - builder.addBooleanProperty("isLimiting", this::isLimiting, null); - builder.addDoubleProperty("powerTime", this::getPowerTime, null); - builder.addBooleanProperty("running", this::isRunning, this::setRunning); - builder.addDoubleProperty("totalPower", this.getGetTotalCurrent(), null); - builder.addDoubleProperty("updateDelay", this::getUpdateDelay, - value -> setUpdateDelay(Math.toIntExact(Math.round(value)))); - builder.addDoubleProperty("voltageDipLow", this::getVoltageDipLow, this::setVoltageDipLow); - builder.addDoubleProperty("voltageMargin", this::getVoltageMargin, this::setVoltageMargin); - builder.addDoubleProperty("voltageDipLength", this::getVoltageDipLength, - this::setVoltageDipLength); - builder.addDoubleProperty("voltageTarget", this::getVoltageTarget, this::setVoltageTarget); - } - -} diff --git a/src/main/java/org/team1540/rooster/power/PowerTelemetry.java b/src/main/java/org/team1540/rooster/power/PowerTelemetry.java deleted file mode 100644 index df976e33..00000000 --- a/src/main/java/org/team1540/rooster/power/PowerTelemetry.java +++ /dev/null @@ -1,23 +0,0 @@ -package org.team1540.rooster.power; - -public interface PowerTelemetry { - - /** - * Gets the total current draw. - * - * @return The current draw in amps. - */ - double getCurrent(); - - /** - * Gets the average voltage. - * - * @return The average voltage in volts. - */ - double getVoltage(); - - default double getPower() { - return getCurrent() * getVoltage(); - } - -} diff --git a/src/main/java/org/team1540/rooster/power/package-info.java b/src/main/java/org/team1540/rooster/power/package-info.java deleted file mode 100644 index 13667ab6..00000000 --- a/src/main/java/org/team1540/rooster/power/package-info.java +++ /dev/null @@ -1,4 +0,0 @@ -/** - * A flexible, dynamic power management system. - */ -package org.team1540.rooster.power; diff --git a/src/main/java/org/team1540/rooster/preferencemanager/PreferenceManager.java b/src/main/java/org/team1540/rooster/preferencemanager/PreferenceManager.java index 8434cfd6..5d9048bc 100644 --- a/src/main/java/org/team1540/rooster/preferencemanager/PreferenceManager.java +++ b/src/main/java/org/team1540/rooster/preferencemanager/PreferenceManager.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Preferences; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.RunCommand; import java.lang.reflect.Field; import java.util.LinkedList; import java.util.List; @@ -11,7 +12,6 @@ import java.util.function.BiFunction; import org.jetbrains.annotations.Contract; import org.jetbrains.annotations.NotNull; -import org.team1540.rooster.util.SimpleLoopCommand; /** * Class to manage creating and updating robot preferences. Add an object containing fields marked @@ -28,10 +28,14 @@ public class PreferenceManager { private boolean enabled = true; private PreferenceManager() { - SimpleLoopCommand managerUpdate = new SimpleLoopCommand("PreferenceManager Update", - this::run); - managerUpdate.setRunWhenDisabled(true); - managerUpdate.start(); + var managerUpdate = new RunCommand(this::run) { + @Override + public boolean runsWhenDisabled() { + return true; + } + }; + managerUpdate.setName("PreferenceManager Update"); + managerUpdate.schedule(); } @Contract(pure = true) diff --git a/src/main/java/org/team1540/rooster/testers/AbstractTester.java b/src/main/java/org/team1540/rooster/testers/AbstractTester.java deleted file mode 100644 index 56eabb27..00000000 --- a/src/main/java/org/team1540/rooster/testers/AbstractTester.java +++ /dev/null @@ -1,209 +0,0 @@ -package org.team1540.rooster.testers; - -import com.google.common.collect.EvictingQueue; -import java.util.Collections; -import java.util.HashMap; -import java.util.List; -import java.util.Map; -import java.util.function.Function; -import org.jetbrains.annotations.NotNull; -import org.jetbrains.annotations.Nullable; - -/** - * A basic implementation of the Tester interface using essentially EvictingQueues for result - * storage. - */ -@SuppressWarnings({"unused", "UnstableApiUsage"}) -public abstract class AbstractTester implements Tester { - - private static float DEFAULT_LOG_TIME = 150; - private static int DEFAULT_UPDATE_DELAY = 2500; - - private int updateDelay; - private boolean running = true; - @NotNull - private List itemsToTest; - @NotNull - private Function test; - @Nullable - private List> runConditions; - @NotNull - private Map> storedResults; - - /** - * Construct a new instance with the default queue depth. - * - * @param test The test to execute. - * @param itemsToTest The items to apply the test to. - * @param runConditions The conditions that must be met before the test will be executed on an - * item. - */ - protected AbstractTester(@NotNull Function test, @NotNull List itemsToTest, - @Nullable List> runConditions) { - this(test, itemsToTest, runConditions, (int) DEFAULT_LOG_TIME / (DEFAULT_UPDATE_DELAY / 1000)); - } - - /** - * Construct a new instance, specifying the logTime and updateDelay to calculate the queue depth. - * - * @param test The test to execute. - * @param itemsToTest The items to apply the test to. - * @param runConditions The conditions that must be met before the test will be executed on an - * item. - * @param logTime The maximum length of time for which we want to store the results. Note that - * this is just used for estimating the queue depth based on the update delay, not actually - * checked against while running. - * @param updateDelay The delay between the test being run on the items. - */ - 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; - } - - /** - * - * Construct a new instance with a given queueDepth. - * @param test The test to execute. - * @param itemsToTest The items to apply the test to. - * @param runConditions The conditions that must be met before the test will be executed on an - * item. - * @param queueDepth The maximum number of items that the {@link EvictingQueue} can hold. - */ - @SuppressWarnings("WeakerAccess") - protected AbstractTester(@NotNull Function test, @NotNull List itemsToTest, - @Nullable List> runConditions, int queueDepth) { - this.test = test; - this.itemsToTest = itemsToTest; - this.runConditions = runConditions; - this.storedResults = new HashMap<>(itemsToTest.size()); - // TODO I feel like there's a cleaner way of doing this - for (T t : itemsToTest) { - this.storedResults.put(t, new ResultStorage<>(queueDepth)); - } - } - - @Override - @NotNull - public Function getTest() { - return test; - } - - @Override - public void setTest(@NotNull Function test) { - this.test = test; - } - - @Override - @Nullable - public List> getRunConditions() { - return runConditions; - } - - @Override - public void setRunConditions( - @Nullable List> runConditions) { - this.runConditions = runConditions; - } - - - @Override - @NotNull - public List getItemsToTest() { - return Collections.unmodifiableList(itemsToTest); - } - - @Override - @NotNull - public EvictingQueue> getStoredResults(T key) { - return storedResults.get(key).queuedResults; - } - - @Override - @Nullable - public ResultWithMetadata peekMostRecentResult(T key) { - return storedResults.get(key).lastResult; - } - - @Override - public int getUpdateDelay() { - return updateDelay; - } - - @Override - public int setUpdateDelay(int delay) { - int oldUpdateDelay = this.updateDelay; - this.updateDelay = delay; - return oldUpdateDelay; - } - - @Override - public boolean setRunning(boolean status) { - boolean oldRunning = this.running; - this.running = status; - return 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!) - */ - protected void periodic() { - for (T t : itemsToTest) { - // 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()); - } - } - - @Override - public void run() { - while (true) { - - if (running) { - periodic(); - } - - try { - Thread.sleep(updateDelay); - } catch (InterruptedException e) { - // End the thread - return; - } - } - } - - /** - * A class for handling the storage of results. Basically just so the tail can actually be - * peeked at. - * @param The returned type to store. - */ - private class ResultStorage { - - @Nullable - private ResultWithMetadata lastResult; - @NotNull - private EvictingQueue> queuedResults; - - private ResultStorage(int queueDepth) { - this.queuedResults = EvictingQueue.create(queueDepth); - } - - private void addResult(A result, long timeStampMillis) { - ResultWithMetadata resultWithMetadata = new ResultWithMetadata<>(result, timeStampMillis); - this.lastResult = resultWithMetadata; - queuedResults.add(resultWithMetadata); - } - - } - -} diff --git a/src/main/java/org/team1540/rooster/testers/ResultWithMetadata.java b/src/main/java/org/team1540/rooster/testers/ResultWithMetadata.java deleted file mode 100644 index ef0be5ed..00000000 --- a/src/main/java/org/team1540/rooster/testers/ResultWithMetadata.java +++ /dev/null @@ -1,32 +0,0 @@ -package org.team1540.rooster.testers; - -import org.jetbrains.annotations.NotNull; - -/** - * Class for encapsulating results. Includes the result and a timestamp. - * - * @param The result type. - */ -public class ResultWithMetadata { - - @NotNull - private final R result; - private final long timestampMillis; - - ResultWithMetadata(@NotNull R result, long timestampMillis) { - this.result = result; - this.timestampMillis = timestampMillis; - } - - @SuppressWarnings("WeakerAccess") - @NotNull - public R getResult() { - return result; - } - - @SuppressWarnings("unused") - public long getTimestampMillis() { - return timestampMillis; - } - -} diff --git a/src/main/java/org/team1540/rooster/testers/Tester.java b/src/main/java/org/team1540/rooster/testers/Tester.java deleted file mode 100644 index 51e98dae..00000000 --- a/src/main/java/org/team1540/rooster/testers/Tester.java +++ /dev/null @@ -1,91 +0,0 @@ -package org.team1540.rooster.testers; - -import com.google.common.collect.EvictingQueue; -import java.util.List; -import java.util.function.Function; -import org.jetbrains.annotations.NotNull; -import org.jetbrains.annotations.Nullable; - -/** - * An interface for testing things. - * - * @param The type of item being tested. - * @param The return type of the test results. - */ -@SuppressWarnings("unused") -public interface Tester extends Runnable { - - @NotNull - Function getTest(); - - void setTest(@NotNull Function tests); - - /** - * Gets the conditions that must be met before the test will be executed on an item. - * - * @return An potentially {@link List} of the run conditions, or null if there are none (always - * execute.) - */ - @SuppressWarnings("UnstableApiUsage") - @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. - * - * @return An {@link EvictingQueue} of the items that are being tested. - */ - @SuppressWarnings("UnstableApiUsage") - @NotNull // TODO how to annotate as unmodifiable? - List getItemsToTest(); - - /** - * Get the results of the test being run. - * - * @param key The item to get the results for. - * @return An {@link EvictingQueue} of {@link ResultWithMetadata} that encapsulate the returned - * values. - */ - @SuppressWarnings("UnstableApiUsage") - @NotNull - EvictingQueue> getStoredResults(T key); - - /** - * Gets the most recent result of the the test without impacting the stored results. - * - * @param key The item to get the results for. - * @return A {@link ResultWithMetadata} that encapsulate the returned value. - */ - @Nullable - ResultWithMetadata peekMostRecentResult(T key); - - /** - * Gets the delay between the test being run on the items. - * @return The delay in milliseconds. - */ - int getUpdateDelay(); - - /** - * Set the delay between the test being run on the items. - * @param delay The new delay in milliseconds. - * @return The previous delay in milliseconds. - */ - @SuppressWarnings("UnusedReturnValue") - int setUpdateDelay(int delay); - - /** - * Set if the tests should be running. Note that this does not stop the thread, only the actual - * execution of the tests. - * @param status The new status. - * @return The old status. - */ - boolean setRunning(boolean status); -} diff --git a/src/main/java/org/team1540/rooster/testers/motor/BurnoutTester.java b/src/main/java/org/team1540/rooster/testers/motor/BurnoutTester.java deleted file mode 100644 index 19c24b61..00000000 --- a/src/main/java/org/team1540/rooster/testers/motor/BurnoutTester.java +++ /dev/null @@ -1,167 +0,0 @@ -package org.team1540.rooster.testers.motor; - -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; -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 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(ChickenTalon)} if there is more - * than one motor and {@link BurnoutTester#testBurnoutSingleMotor(ChickenTalon)} 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(ChickenTalon... 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(ChickenTalon)} if there are more - * than two motor sand {@link BurnoutTester#testBurnoutSingleMotor(ChickenTalon)} 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(ChickenTalon 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(ChickenTalon 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(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; - } - - /** - * 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) { - for (ChickenTalon t : getItemsToTest()) { - // Get the most recent value if present, else simply don't add it to the builder - builder.addBooleanProperty(t.getDeviceID() + "", - () -> Optional.ofNullable(peekMostRecentResult(t)) - .map(ResultWithMetadata::getResult).orElse(false), null); - } - builder.addDoubleProperty("Median current", () -> medianCurrent, null); - } -} diff --git a/src/main/java/org/team1540/rooster/testers/motor/ControllersMultiTester.java b/src/main/java/org/team1540/rooster/testers/motor/ControllersMultiTester.java deleted file mode 100644 index 3214968b..00000000 --- a/src/main/java/org/team1540/rooster/testers/motor/ControllersMultiTester.java +++ /dev/null @@ -1,209 +0,0 @@ -package org.team1540.rooster.testers.motor; - -import com.ctre.phoenix.motorcontrol.ControlMode; -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. - */ -@SuppressWarnings("unused") -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(ChickenTalon...)} 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 (ChickenTalon 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 (ChickenTalon 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 - */ - @SuppressWarnings("WeakerAccess") - public ControllersMultiTester addControllerGroup(ChickenTalon... 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, - ChickenTalon... 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 - */ - @SuppressWarnings("WeakerAccess") - 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 (ChickenTalon 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(ChickenTalon 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/src/main/java/org/team1540/rooster/testers/motor/EncoderTester.java b/src/main/java/org/team1540/rooster/testers/motor/EncoderTester.java deleted file mode 100644 index 4b4d9805..00000000 --- a/src/main/java/org/team1540/rooster/testers/motor/EncoderTester.java +++ /dev/null @@ -1,137 +0,0 @@ -package org.team1540.rooster.testers.motor; - -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; -import org.team1540.rooster.wrappers.ChickenTalon; - -/** - * 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 ChickenTalon ChickenTalons} to compare to each other. - */ - @SuppressWarnings("WeakerAccess") - public EncoderTester(ChickenTalon... 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 ChickenTalon ChickenTalons} 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 if telemetry is available, or else, - 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 ChickenTalon} is moving at a velocity of less than 5. - * - * @param controller The {@link ChickenTalon} 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(ChickenTalon 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 encoder 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 encoder 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 ChickenTalon}. - * - * @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() + "", - () -> Optional.ofNullable(peekMostRecentResult(t)) - .map(ResultWithMetadata::getResult).orElse(false), null); - } - } -} diff --git a/src/main/java/org/team1540/rooster/testers/motor/SimpleControllersTester.java b/src/main/java/org/team1540/rooster/testers/motor/SimpleControllersTester.java deleted file mode 100644 index 967287c5..00000000 --- a/src/main/java/org/team1540/rooster/testers/motor/SimpleControllersTester.java +++ /dev/null @@ -1,166 +0,0 @@ -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; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import java.util.NavigableMap; -import java.util.Optional; -import java.util.TreeMap; -import org.jetbrains.annotations.Contract; -import org.team1540.rooster.util.SimpleCommand; - -/** - * A simple command for testing a series of {@link IMotorController IMotorControllers}. Simply - * add the controllers, specify control bindings or use the defaults, and you're ready! Select - * the motor in the SmartDashboard or move next/previous with buttons and control the percent - * output of the motor with a joystick axis. - */ -@SuppressWarnings("unused") -public class SimpleControllersTester extends Command implements Sendable { - - private static final int DEFAULT_JOYSTICK_ID = 0; - private static final int DEFAULT_AXIS_ID = 5; - private static final int DEFAULT_NEXT_BUTTON_ID = 1; - private static final int DEFAULT_PREVIOUS_BUTTON_ID = 2; - - private Joystick joystick; - private int axisId; - - // Is the retrieval time poor, despite the need for a lot of retrievals? Yes. Are we ignoring that - // because the size of the map is small? Also yes. - private NavigableMap controllers = new TreeMap<>(); - - // Do not manually update this value. Instead, call setCurrentController() - private IMotorController currentController; - - private SendableChooser controllerChooser = new SendableChooser<>(); - - /** - * Construct a new instance with the default bindings on {@link Joystick} 0. On an Xbox - * controller, the right thumbstick y-axis controls {@link ControlMode}.PERCENT_OUTPUT, the A - * button goes to the next {@link IMotorController}, and the B button goes to the previous - * {@link IMotorController}. - * - * @param controllers The {@link IMotorController IMotorControllers} to test. - */ - @SuppressWarnings({"SpellCheckingInspection", "unused"}) - public SimpleControllersTester(IMotorController... controllers) { - this(new Joystick(DEFAULT_JOYSTICK_ID), DEFAULT_AXIS_ID, DEFAULT_NEXT_BUTTON_ID, - DEFAULT_PREVIOUS_BUTTON_ID, controllers); - } - - /** - * Construct a new instance with the specified bindings. - * @param joystick The joystick port to use. - * @param axisId The axis on the joystick to use. - * @param nextButtonId The button to use to proceed to the next {@link IMotorController}. - * @param previousButtonId The button to use to proceed to the nex {@link IMotorController}. - * @param controllers The {@link IMotorController IMotorControllers} to test. - */ - @SuppressWarnings("WeakerAccess") - public SimpleControllersTester(Joystick joystick, int axisId, int nextButtonId, - int previousButtonId, - IMotorController... controllers) { - this.joystick = joystick; - this.axisId = axisId; - - // Make the buttons go the next and previous controller, or loop around if not available - new JoystickButton(this.joystick, nextButtonId).whenPressed(new SimpleCommand("Next " - + "controller ID", - () -> setCurrentController( - Optional.ofNullable(this.controllers.higherKey(this.currentController.getDeviceID())) - .orElse(this.controllers.firstKey())))); - new JoystickButton(this.joystick, previousButtonId).whenPressed(new SimpleCommand("Previous " - + "controller ID", - () -> setCurrentController( - Optional.ofNullable(this.controllers.lowerKey(this.currentController.getDeviceID())) - .orElse(this.controllers.lastKey())))); - - // Add a chooser that you can use to select the controller and initialize it to null, - // corresponding with buttons should be used - this.controllerChooser.addOption("Use buttons", null); - for (IMotorController controller : controllers) { - this.controllers.put(controller.getDeviceID(), controller); - this.controllerChooser.addOption(String.valueOf(controller.getDeviceID()), - controller.getDeviceID()); - controller.setNeutralMode(NeutralMode.Coast); - } - - // Set the active controller to the first controller - setCurrentController(controllers[0].getDeviceID()); - } - - /** - * Checks every tick if the chooser is set to a controller or to use buttons, then sets the - * output according to the value of the active joystick. - */ - @Override - protected void execute() { - if (controllerChooser.getSelected() != null) { - setCurrentController(controllerChooser.getSelected()); - } - currentController.set(ControlMode.PercentOutput, joystick.getRawAxis(axisId)); - } - - /** - * Updates the currently active {@link IMotorController}. - * @param newId The ID of the new {@link IMotorController}. - */ - @SuppressWarnings("WeakerAccess") - public void setCurrentController(int newId) { - if (currentController != null) { - currentController.set(ControlMode.PercentOutput, 0); - } - currentController = controllers.get(newId); - } - - /** - * Prevents the command from finishing. - * @return false. - */ - @Override - protected boolean isFinished() { - return false; - } - - /** - * Gets the chooser used for selecting the current {@link IMotorController}. - * @return A chooser. - */ - @SuppressWarnings("WeakerAccess") - public SendableChooser getControllerChooser() { - return controllerChooser; - } - - /** - * Adds a single property showing the current active controller. - * @param builder The thing to add the things to. - */ - @Override - public void initSendable(SendableBuilder builder) { - builder.addDoubleProperty("Controller ID", () -> this.currentController.getDeviceID(), - (id) -> setCurrentController((int) id)); - } - - /** - * Add all the sendables to the {@link SmartDashboard}. Includes chooser for selecting the - * active {@link IMotorController} and information about the tester. Follows a builder pattern. - * @return This {@code SimpleControllersTester} in a builder pattern. - */ - @SuppressWarnings({"UnusedReturnValue", "SpellCheckingInspection"}) - @Contract(" -> this") - public SimpleControllersTester addAllSendables() { - SmartDashboard.putData("Controller tester info", this); - SmartDashboard.putData("Controller choose", getControllerChooser()); - return this; - } - - -} diff --git a/src/main/java/org/team1540/rooster/testers/package-info.java b/src/main/java/org/team1540/rooster/testers/package-info.java deleted file mode 100644 index 33dbb24a..00000000 --- a/src/main/java/org/team1540/rooster/testers/package-info.java +++ /dev/null @@ -1,4 +0,0 @@ -/** - * Various classes for testing common things. - */ -package org.team1540.rooster.testers; diff --git a/src/main/java/org/team1540/rooster/triggers/AxisButton.java b/src/main/java/org/team1540/rooster/triggers/AxisButton.java index deda117d..68f2f990 100644 --- a/src/main/java/org/team1540/rooster/triggers/AxisButton.java +++ b/src/main/java/org/team1540/rooster/triggers/AxisButton.java @@ -1,7 +1,6 @@ package org.team1540.rooster.triggers; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.buttons.Button; import java.util.Objects; import org.jetbrains.annotations.NotNull; @@ -9,7 +8,7 @@ * A button based on a joystick axis. This can be used to emulate a button using a trigger or * joystick. */ -public class AxisButton extends Button { +public class AxisButton { private GenericHID stick; private double threshold; @@ -29,7 +28,7 @@ public AxisButton(@NotNull GenericHID stick, double threshold, int axis) { this.axis = axis; } - @Override + // @Override public boolean get() { return (Math.abs(stick.getRawAxis(axis)) >= Math.abs(threshold) && Math.signum(stick.getRawAxis(axis)) == Math.signum(threshold)); diff --git a/src/main/java/org/team1540/rooster/triggers/DPadButton.java b/src/main/java/org/team1540/rooster/triggers/DPadButton.java index f97ea0ff..f3dd3b32 100644 --- a/src/main/java/org/team1540/rooster/triggers/DPadButton.java +++ b/src/main/java/org/team1540/rooster/triggers/DPadButton.java @@ -1,7 +1,7 @@ package org.team1540.rooster.triggers; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.buttons.Button; +import edu.wpi.first.wpilibj2.command.button.Button; import java.util.Objects; import org.jetbrains.annotations.Contract; import org.jetbrains.annotations.NotNull; diff --git a/src/main/java/org/team1540/rooster/triggers/MultiAxisButton.java b/src/main/java/org/team1540/rooster/triggers/MultiAxisButton.java index c6d71f92..14348f2d 100644 --- a/src/main/java/org/team1540/rooster/triggers/MultiAxisButton.java +++ b/src/main/java/org/team1540/rooster/triggers/MultiAxisButton.java @@ -1,10 +1,9 @@ package org.team1540.rooster.triggers; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.buttons.Button; -import org.jetbrains.annotations.NotNull; - +import edu.wpi.first.wpilibj2.command.button.Button; import java.util.Objects; +import org.jetbrains.annotations.NotNull; /** * A button based on a set of joystick axes. diff --git a/src/main/java/org/team1540/rooster/triggers/SimpleButton.java b/src/main/java/org/team1540/rooster/triggers/SimpleButton.java deleted file mode 100644 index 07c2dbed..00000000 --- a/src/main/java/org/team1540/rooster/triggers/SimpleButton.java +++ /dev/null @@ -1,43 +0,0 @@ -package org.team1540.rooster.triggers; - -import edu.wpi.first.wpilibj.buttons.Button; -import java.util.Objects; -import java.util.function.BooleanSupplier; -import org.jetbrains.annotations.Contract; -import org.jetbrains.annotations.NotNull; - -/** - * A simple {@link Button} that can be constructed with a lambda method or function reference that - * supplies a {@code boolean}. - */ -public class SimpleButton extends Button { - - private BooleanSupplier supplier; - - /** - * Constructs a new {@code SimpleButton} that uses the provided {@link BooleanSupplier} to - * determine its state. - * - * @param supplier The {@code BooleanSupplier} to call for the button's state. - * @throws NullPointerException If {@code supplier} is {@code null}. - */ - public SimpleButton(@NotNull BooleanSupplier supplier) { - this.supplier = Objects.requireNonNull(supplier); - } - - @Override - @Contract(pure = true) - public boolean get() { - return supplier.getAsBoolean(); - } - - @NotNull - @Contract(pure = true) - public BooleanSupplier getSupplier() { - return supplier; - } - - public void setSupplier(@NotNull BooleanSupplier supplier) { - this.supplier = supplier; - } -} diff --git a/src/main/java/org/team1540/rooster/triggers/StrictDPadButton.java b/src/main/java/org/team1540/rooster/triggers/StrictDPadButton.java index 6267f466..d629d704 100644 --- a/src/main/java/org/team1540/rooster/triggers/StrictDPadButton.java +++ b/src/main/java/org/team1540/rooster/triggers/StrictDPadButton.java @@ -1,7 +1,7 @@ package org.team1540.rooster.triggers; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.buttons.Button; +import edu.wpi.first.wpilibj2.command.button.Button; import java.util.Objects; import org.jetbrains.annotations.Contract; import org.jetbrains.annotations.NotNull; diff --git a/src/main/java/org/team1540/rooster/triggers/package-info.java b/src/main/java/org/team1540/rooster/triggers/package-info.java index 06925f7d..64281221 100644 --- a/src/main/java/org/team1540/rooster/triggers/package-info.java +++ b/src/main/java/org/team1540/rooster/triggers/package-info.java @@ -1,5 +1,5 @@ /** - * Extensions of WPILib {@link edu.wpi.first.wpilibj.buttons.Button} to support various buttons not - * covered by stock WPILib. + * Extensions of WPILib {@link edu.wpi.first.wpilibj2.command.button.Button Buttons} to support + * various buttons not covered by stock WPILib. */ package org.team1540.rooster.triggers; diff --git a/src/main/java/org/team1540/rooster/util/AsyncCommand.java b/src/main/java/org/team1540/rooster/util/AsyncCommand.java deleted file mode 100644 index 4891cfaf..00000000 --- a/src/main/java/org/team1540/rooster/util/AsyncCommand.java +++ /dev/null @@ -1,131 +0,0 @@ -package org.team1540.rooster.util; - -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.command.Command; -import org.jetbrains.annotations.Contract; - -/** - * A {@link Command} that spawns off a new {@link Notifier} on initialization and stops it. That - * thread will call the {@link #runPeriodic()} ()} method at a user-specified interval with precise - * timings. - * - * @see Thread - */ -public abstract class AsyncCommand extends Command { - - private double interval; - private boolean finished = false; - - private Notifier notifier; - - /** - * Constructs a new {@code AsyncCommand} with a preset periodic interval. - * - * @param interval The interval between {@link #runPeriodic()} ()} calls, in milliseconds. - */ - public AsyncCommand(long interval) { - this.interval = interval / 1000.0; - } - - /** - * Runs the action of this command. - *

- * This is equivalent to the {@link #execute()} method in normal commands. Note that this method - * will be called in a seperate thread, and as such any public methods it calls, fields it uses, - * etc. MUST be thread-safe. - */ - protected void runPeriodic() { - - } - - /** - * Runs initialization code for this command. - * - * This is equivalent to the {@link Command#initialize()} method. - */ - protected void runInitial() { - - } - - /** - * Called when the {@code AsyncCommand} ends peacefully. This is equivalent to {@link - * Command#end()}. - * - * This method is called when the command ends peacefully, i.e. when {@link #markAsFinished()} was - * called. However, it is possible that another command interrupts this command after {@link - * #markAsFinished()} was called but before the command was removed from the scheduler, and as - * such this method might not be called even if {@link #markAsFinished()} was called. - */ - protected void runEnd() { - - } - - /** - * Called when the {@code AsyncCommand} is interrupted. - * - * This method serves an identical purpose to {@link Command#interrupted()}. By default, this - * method calls {@link #runEnd()}. - */ - protected void runInterrupt() { - runEnd(); - } - - @Override - protected final void initialize() { - finished = false; - runInitial(); - - notifier = new Notifier(() -> { - if (!finished) { - runPeriodic(); - } - }); - - notifier.startPeriodic(interval); - } - - @Override - protected final void interrupted() { - notifier.stop(); - runInterrupt(); - } - - @Override - protected final void end() { - notifier.stop(); - runEnd(); - } - - @Override - @Contract(pure = true) - protected boolean isFinished() { - return finished; - } - - /** - * Marks the command as finished. The {@link #runPeriodic()} method will no longer be called, and the - * command will be canceled on the next scheduler tick. - */ - protected void markAsFinished() { - this.finished = true; - } - - /** - * Gets the periodic interval between calls to {@link #runPeriodic()}. - * - * @return The interval, in milliseconds. - */ - @Contract(pure = true) - public long getInterval() { - return (long) interval * 1000; - } - - /** - * Sets the periodic interval between calls to {@link #runPeriodic()}. - * - * @param interval The interval, in milliseconds. - */ - public void setInterval(long interval) { - this.interval = interval / 1000.0; - } -} diff --git a/src/main/java/org/team1540/rooster/util/ChickenXboxController.java b/src/main/java/org/team1540/rooster/util/ChickenXboxController.java new file mode 100644 index 00000000..f1fdf6b1 --- /dev/null +++ b/src/main/java/org/team1540/rooster/util/ChickenXboxController.java @@ -0,0 +1,232 @@ +package org.team1540.rooster.util; + +import static org.team1540.rooster.util.ControlUtils.deadzone; +import static org.team1540.rooster.util.MathUtils.preserveSignRaiseToPower; + +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import java.util.HashMap; +import java.util.Map; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; +import org.team1540.rooster.triggers.AxisButton; +import org.team1540.rooster.triggers.DPadAxis; +import org.team1540.rooster.triggers.MultiAxisButton; +import org.team1540.rooster.triggers.StrictDPadButton; + +public class ChickenXboxController extends XboxController { + + /** + * Construct an instance of a joystick. The joystick index is the USB port on the drivers + * station. + * + * @param port The port on the Driver Station that the joystick is plugged into. + */ + public ChickenXboxController(int port) { + super(port); + } + + public enum XboxAxis { + LEFT_X(0), + LEFT_Y(1), + LEFT_TRIG(2), + RIGHT_TRIG(3), + RIGHT_X(4), + RIGHT_Y(5); + + @SuppressWarnings("MemberName") + public final int value; + @SuppressWarnings("PMD.UseConcurrentHashMap") + private static final Map map = new HashMap<>(); + + XboxAxis(int value) { + this.value = value; + } + + static { + for (XboxAxis axisId : XboxAxis.values()) { + map.put(axisId.value, axisId); + } + } + + public static XboxAxis of(int value) { + return map.get(value); + } + } + + public enum XboxButton { // Hmm, I know about XboxController.Button but like... + A(1), + B(2), + X(3), + Y(4), + LB(5), + RB(6), + BACK(7), + START(8), + LEFT_PRESS(9), + RIGHT_PRESS(10); + + @SuppressWarnings("MemberName") + public final int value; + @SuppressWarnings("PMD.UseConcurrentHashMap") + private static final Map map = new HashMap<>(); + + XboxButton(int value) { + this.value = value; + } + + static { + for (XboxButton axisId : XboxButton.values()) { + map.put(axisId.value, axisId); + } + } + + public static XboxButton of(int value) { + return map.get(value); + } + } + + public double getRawAxis(XboxAxis axis) { + return super.getRawAxis(axis.value); + } + + /** + * Get the X axis value of the controller in the official 1540 coordinate system + * + * @param hand Side of controller whose value should be returned. + * @return The X axis value of the controller in the official 1540 coordinate system + */ + public double getRectifiedX(Hand hand) { + return -super.getY(hand); + } + + /** + * Get the Y axis value of the controller in the official 1540 coordinate system + * + * @param hand Side of controller whose value should be returned. + * @return The Y axis value of the controller in the official 1540 coordinate system + */ + public double getRectifiedY(Hand hand) { + return -super.getX(hand); + } + + public Vector2D get2DJoystickVector(Hand hand) { + return new Vector2D(getRectifiedX(hand), getRectifiedY(hand)); + } + + /** + * Gets angle from a 2D joystick + * + * @param hand Left vs right joystick of the xbox this + * @return Angle in radians counter-clockwise from 12 o'clock + */ + public double get2DJoystickAngle(Hand hand) { + return Math.atan2(getRectifiedY(hand), getRectifiedX(hand)); + } + + public double get2DJoystickMagnitude(Hand hand) { + return Vector2D.ZERO.distance(get2DJoystickVector(hand)); + } + + public StrictDPadButton getButton(DPadAxis button) { + return new StrictDPadButton(this, 0, button); + } + + public JoystickButton getButton(XboxButton button) { + return new JoystickButton(this, button.value); + } + + public AxisButton getButton(XboxAxis axis, double threshold) { + return new AxisButton(this, threshold, axis.value); + } + + public MultiAxisButton getButton(double threshold, XboxAxis... axes) { + int[] axesIds = new int[axes.length]; + for (int i = 0; i < axes.length; i++) { + axesIds[i] = axes[i].value; + } + return new MultiAxisButton(this, threshold, axesIds); + } + + public Axis getAxis(XboxAxis axis) { + return () -> getRawAxis(axis); + } + + public Axis getAxis(int axis) { + return () -> getRawAxis(axis); + } + + public Axis getXAxis(Hand hand) { + return () -> getX(hand); + } + + public Axis getYAxis(Hand hand) { + return () -> getY(hand); + } + + public Axis getRectifiedXAxis(Hand hand) { + return getYAxis(hand).inverted(); + } + + public Axis getRectifiedYAxis(Hand hand) { + return getXAxis(hand).inverted(); + } + + public Axis2D getJoystick(Hand hand) { + return () -> get2DJoystickVector(hand); + } + + public interface Axis extends DoubleSupplier { + + default Axis withDeadzone(double deadzone) { + return () -> deadzone(value(), deadzone); + } + + default Axis powerScaled(double power) { + return () -> preserveSignRaiseToPower(value(), power); + } + + default Axis inverted() { + return () -> -value(); + } + + default Axis2D withYAxis(Axis axis) { + return () -> new Vector2D(value(), axis.value()); + } + + default Axis2D withXAxis(Axis axis) { + return () -> new Vector2D(axis.value(), value()); + } + + default double value() { + return getAsDouble(); + } + } + + public interface Axis2D extends Supplier { + + default Axis magnitude() { + return () -> value().getNorm(); + } + + default Axis angle() { + return () -> { + Vector2D value = value(); // to avoid creating the object twice + return Math.atan2(value.getY(), value.getX()); + }; + } + + default Axis x() { + return () -> value().getX(); + } + + default Axis y() { + return () -> value().getY(); + } + + default Vector2D value() { + return get(); + } + } +} diff --git a/src/main/java/org/team1540/rooster/util/ControlUtils.java b/src/main/java/org/team1540/rooster/util/ControlUtils.java new file mode 100644 index 00000000..55474dad --- /dev/null +++ b/src/main/java/org/team1540/rooster/util/ControlUtils.java @@ -0,0 +1,84 @@ +package org.team1540.rooster.util; + +import org.jetbrains.annotations.Contract; + +public class ControlUtils { + + public static double constrainAbsValue(double velocity, double maxVelocity, + double minVelocity) { + if (Math.abs(velocity) > maxVelocity) { + velocity = Math.copySign(maxVelocity, velocity); + } else if (Math.abs(velocity) < minVelocity) { + velocity = Math.copySign(minVelocity, velocity); + } + return velocity; + } + + /** + * Processes an axis and returns the value only if it is outside the provided deadzone. + * + * @param axis The axis to return. + * @param deadzone The deadzone to use. + * @return If |{@code axis}| is greater than |{@code deadzone}|, returns {@code axis}; + * otherwise, returns 0. + */ + public static double simpleDeadzone(double axis, double deadzone) { + return (Math.abs(axis) > Math.abs(deadzone)) ? axis : 0; + } + + public static double constrainAndDeadzone(double output, double max, double min, + double deadzone) { + return simpleDeadzone(constrainAbsValue(output, max, min), deadzone); + } + + /** + * https://www.desmos.com/calculator/ybuyhcfzgm + * + * @param input x + * @param fast f + * @param slow s + * @param fastX b, fastX < slowX + * @param slowX a + * @return y + */ + public static double deadzone(double input, boolean allowNegativeValues, double fast, + double slow, double fastX, double slowX) { + double slope = (fast - slow) / (fastX - slowX); + double absInput = Math.abs(input); + double rawResult = slope * (absInput - slowX) + slow; + if (absInput < slowX) { + rawResult = slow; + } else if (absInput > fastX) { + rawResult = fast; + } + rawResult = Math.copySign(rawResult, input); + if (allowNegativeValues) { + return rawResult; + } + if (rawResult < 0) { + rawResult = slow; + } + return rawResult; + } + + /** + * Processes an axis with a deadzone. This implementation scales the output such that the area + * between the deadzone and 1 is mapped to the full range of motion. + *

+ * Full implementation details: The function processes the deadzone according to the function + * \(\DeclareMathOperator{\sgn}{sgn}\frac{d(x)-Z\sgn(d(x))}{1-Z}\), where \(x\) is the {@code + * axis} parameter, \(Z\) is the {@code deadzone} parameter, and \(d(x)\) is the deadzone + * function \(d(x)=\begin{cases}x & |x| \geq D \\ 0 & |x| < D\end{cases}\). (Note: + * Equations may require viewing the Javadoc using a browser.) + * + * @param axis The axis to return. + * @param deadzone The deadzone to use. + * @return The axis value processed with respect to the specified deadzone. + */ + @Contract(pure = true) + public static double deadzone(double axis, double deadzone) { + double baseDeadzone = (Math.abs(axis) > Math.abs(deadzone)) ? axis : 0; + return baseDeadzone != 0 ? (baseDeadzone - Math.copySign(deadzone, baseDeadzone)) / (1 + - deadzone) : 0; + } +} diff --git a/src/main/java/org/team1540/rooster/util/DashboardUtils.java b/src/main/java/org/team1540/rooster/util/DashboardUtils.java deleted file mode 100644 index fdb67d0a..00000000 --- a/src/main/java/org/team1540/rooster/util/DashboardUtils.java +++ /dev/null @@ -1,49 +0,0 @@ -package org.team1540.rooster.util; - -import edu.wpi.first.wpilibj.Sendable; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import java.util.Objects; -import org.jetbrains.annotations.NotNull; -import org.team1540.rooster.functional.Executable; -import org.team1540.rooster.preferencemanager.PreferenceManager; - -/** - * A collection of utilities for the {@link SmartDashboard}. - * - * @see SmartDashboard - * @see PreferenceManager - */ -public class DashboardUtils { - - // should never be instantiated - private DashboardUtils() { - } - - /** - * Adds a button to the {@link SmartDashboard} to run the provided code. - *

- * Internally, this method creates a {@link SimpleCommand} with the provided action and - * requirements and adds it to the dashboard via {@link SmartDashboard#putData(String, Sendable) - * putData()} with the provided label. As this uses the command system, calling {@code - * Scheduler.getInstance().run()} is necessary in your main robot loop for the action to be run - * properly. Like any other command, the action will be run in the main robot thread and thus no - * synchronization is necessary. - * - * @param label The label for the entry on the {@link SmartDashboard}. - * @param buttonAction This {@code Executable}'s {@link Executable#execute() execute()} method - * will be called when the button is pressed. - * @param reqs The {@link Subsystem Subsystems} to be passed into the internal command as - * requirements. - * @see SimpleCommand - * @see Command - * @throws NullPointerException If any parameters are {@code null}. - */ - public static void addDashboardButton(@NotNull String label, @NotNull Executable buttonAction, @NotNull Subsystem... reqs) { - for (Subsystem req : reqs) { - Objects.requireNonNull(req); - } - SmartDashboard.putData(label, new SimpleCommand(Objects.requireNonNull(label), Objects.requireNonNull(buttonAction), reqs)); - } -} diff --git a/src/main/java/org/team1540/rooster/util/Executable.java b/src/main/java/org/team1540/rooster/util/Executable.java deleted file mode 100644 index e69de29b..00000000 diff --git a/src/main/java/org/team1540/rooster/util/FakeGyro.java b/src/main/java/org/team1540/rooster/util/FakeGyro.java new file mode 100644 index 00000000..5301e5e1 --- /dev/null +++ b/src/main/java/org/team1540/rooster/util/FakeGyro.java @@ -0,0 +1,41 @@ +package org.team1540.rooster.util; + +import edu.wpi.first.wpilibj.GyroBase; + +/** + * Mock gyro with user-controllable angle. Useful for displaying an angle measure on Shuffleboard. + * Displays whatever angle (in degrees) is passed into {@link #setAngle(double)}. + */ +public class FakeGyro extends GyroBase { + + private double angle = 0; + + @Override + public void calibrate() { + + } + + @Override + public void reset() { + + } + + public void setAngle(double angle) { + this.angle = angle; + } + + @Override + public double getAngle() { + return angle; + } + + @Override + public double getRate() { + return 0; + } + + @Override + public void close() { + + } +} diff --git a/src/main/java/org/team1540/rooster/util/MathUtils.java b/src/main/java/org/team1540/rooster/util/MathUtils.java new file mode 100644 index 00000000..090024ad --- /dev/null +++ b/src/main/java/org/team1540/rooster/util/MathUtils.java @@ -0,0 +1,79 @@ +package org.team1540.rooster.util; + +import org.jetbrains.annotations.Contract; + +public class MathUtils { + + /** + * Inverts the provided value if {@code shouldInvert} is {@code true}. + * + * @param shouldInvert Whether to invert the number. + * @param toInvert The number to invert. + * @return If {@code shouldInvert} is {@code true}, {@code -toInvert}; otherwise, {@code + * toInvert} + */ + @Contract(pure = true) + public static double negateDoubleIf(boolean shouldInvert, double toInvert) { + return (shouldInvert ? -1 : 1) * toInvert; + } + + /** + * Constrains an input to a given range in either direction from zero. + * + * This does not map the input to the range; it simply hard-caps it when it's + * outside. + * + * @param input The input to constrain. + * @param cap The distance from zero to constrain {@code input} to. + * @return If {@code input} > {@code cap}, return {@code cap}; if {@code input} < {@code + * -cap}, return {@code -cap}; otherwise, return {@code input}. + */ + @Contract(pure = true) + public static double constrain(double input, double cap) { + if (cap < 0) { + throw new IllegalArgumentException("Cap cannot be negative"); + } + + return constrain(input, -cap, cap); + } + + /** + * Constrains an input to a given range. + * + * This does not map the input to the range; it simply hard-caps it when it's + * outside. + * + * @param input The input to constrain. + * @param lowerCap The lower bound of the range. + * @param upperCap The upper bound of the range. + * @return If {@code input} > {@code upperCap}, return {@code upperCap}; if {@code input} + * < {@code lowerCap}, return {@code lowerCap}; otherwise, return {@code input}. + */ + @Contract(pure = true) + public static double constrain(double input, double lowerCap, double upperCap) { + if (lowerCap > upperCap) { + throw new IllegalArgumentException("Lower cap cannot be less than upper cap"); + } + + if (input < lowerCap) { + return lowerCap; + } else if (input > upperCap) { + return upperCap; + } else { + return input; + } + } + + /** + * Raises the input to the provided power while preserving the sign. Useful for joystick + * scaling. + * + * @param input The input to be raised. + * @param pow The power. + * @return The input raised to the provided power, with the sign of the input. + */ + @Contract(pure = true) + public static double preserveSignRaiseToPower(double input, double pow) { + return Math.copySign(Math.pow(Math.abs(input), pow), input); + } +} diff --git a/src/main/java/org/team1540/rooster/util/MiniPID.java b/src/main/java/org/team1540/rooster/util/MiniPID.java new file mode 100644 index 00000000..81309a77 --- /dev/null +++ b/src/main/java/org/team1540/rooster/util/MiniPID.java @@ -0,0 +1,486 @@ +package org.team1540.rooster.util; + +/** + * Source: https://github.com/tekdemo/MiniPID-Java Small, easy to use PID implementation with + * advanced controller capability.
Minimal usage:
MiniPID pid = new MiniPID(p,i,d);
+ * ...looping code...{
output= pid.getOutput(sensorvalue,target);
} + */ +public class MiniPID { + //********************************** + // Class private variables + //********************************** + + private double P = 0; + private double I = 0; + private double D = 0; + private double F = 0; + + private double maxIOutput = 0; + private double maxError = 0; + private double errorSum = 0; + + private double maxOutput = 0; + private double minOutput = 0; + + private double setpoint = 0; + + private double lastActual = 0; + + private boolean firstRun = true; + private boolean reversed = false; + + private double outputRampRate = 0; + private double lastOutput = 0; + + private double outputFilter = 0; + + private double setpointRange = 0; + + //********************************** + // Constructor functions + //********************************** + + /** + * Create a MiniPID class object. See setP, setI, setD methods for more detailed parameters. + * + * @param p Proportional gain. Large if large difference between setpoint and target. + * @param i Integral gain. Becomes large if setpoint cannot reach target quickly. + * @param d Derivative gain. Responds quickly to large changes in error. Small values prevents P + * and I terms from causing overshoot. + */ + public MiniPID(double p, double i, double d) { + P = p; + I = i; + D = d; + checkSigns(); + } + + /** + * Create a MiniPID class object. See setP, setI, setD, setF methods for more detailed + * parameters. + * + * @param p Proportional gain. Large if large difference between setpoint and target. + * @param i Integral gain. Becomes large if setpoint cannot reach target quickly. + * @param d Derivative gain. Responds quickly to large changes in error. Small values prevents P + * and I terms from causing overshoot. + * @param f Feed-forward gain. Open loop "best guess" for the output should be. Only useful if + * setpoint represents a rate. + */ + public MiniPID(double p, double i, double d, double f) { + P = p; + I = i; + D = d; + F = f; + checkSigns(); + } + + //********************************** + // Configuration functions + //********************************** + + /** + * Configure the Proportional gain parameter.
This responds quickly to changes in setpoint, + * and provides most of the initial driving force to make corrections.
Some systems can be + * used with only a P gain, and many can be operated with only PI.
For position based + * controllers, this is the first parameter to tune, with I second.
For rate controlled + * systems, this is often the second after F. + * + * @param p Proportional gain. Affects output according to output+=P*(setpoint-current_value) + */ + public void setP(double p) { + P = p; + checkSigns(); + } + + /** + * Changes the I parameter
This is used for overcoming disturbances, and ensuring that the + * controller always gets to the control mode. Typically tuned second for "Position" based + * modes, and third for "Rate" or continuous based modes.
Affects output through + * output+=previous_errors*Igain ;previous_errors+=current_error + * + * @param i New gain value for the Integral term + */ + public void setI(double i) { + if (I != 0) { + errorSum = errorSum * I / i; + } + if (maxIOutput != 0) { + maxError = maxIOutput / i; + } + I = i; + checkSigns(); + // Implementation note: + // This Scales the accumulated error to avoid output errors. + // As an example doubling the I term cuts the accumulated error in half, which results in the + // output change due to the I term constant during the transition. + } + + /** + * Changes the D parameter
This has two primary effects: + *

+ * A small D value can be useful for both improving response times, and preventing overshoot. However, in many systems a large D value will cause significant instability, particularly for large + * setpoint changes. + *
+ * Affects output through output += -D*(current_input_value - last_input_value) + * + * @param d New gain value for the Derivative term + */ + public void setD(double d) { + D = d; + checkSigns(); + } + + /** + * Configure the FeedForward parameter.
This is excellent for velocity, rate, and other + * continuous control modes where you can expect a rough output value based solely on the + * setpoint.
Should not be used in "position" based control modes.
Affects output + * according to output+=F*Setpoint. Note, that a F-only system is actually open loop. + * + * @param f Feed forward gain. + */ + public void setF(double f) { + F = f; + checkSigns(); + } + + /** + * Configure the PID object. See setP, setI, setD methods for more detailed parameters. + * + * @param p Proportional gain. Large if large difference between setpoint and target. + * @param i Integral gain. Becomes large if setpoint cannot reach target quickly. + * @param d Derivative gain. Responds quickly to large changes in error. Small values prevents P + * and I terms from causing overshoot. + */ + public void setPID(double p, double i, double d) { + P = p; + D = d; + //Note: the I term has additional calculations, so we need to use it's + //specific method for setting it. + setI(i); + checkSigns(); + } + + /** + * Configure the PID object. See setP, setI, setD, setF methods for more detailed parameters. + * + * @param p Proportional gain. Large if large difference between setpoint and target. + * @param i Integral gain. Becomes large if setpoint cannot reach target quickly. + * @param d Derivative gain. Responds quickly to large changes in error. Small values prevents P + * and I terms from causing overshoot. + * @param f Feed-forward gain. Open loop "best guess" for the output should be. Only useful if + * setpoint represents a rate. + */ + public void setPID(double p, double i, double d, double f) { + P = p; + D = d; + F = f; + //Note: the I term has additional calculations, so we need to use it's + //specific method for setting it. + setI(i); + checkSigns(); + } + + /** + * Set the maximum output value contributed by the I component of the system This can be used to + * prevent large windup issues and make tuning simpler + */ + public void setMaxIOutput(double maximum) { + // Internally maxError and Izone are similar, but scaled for different purposes. + // The maxError is generated for simplifying math, since calculations against + // the max error are far more common than changing the I term or Izone. + maxIOutput = maximum; + if (I != 0) { + maxError = maxIOutput / I; + } + } + + /** + * Specify a maximum output range.
When one input is specified, output range is configured + * to + * [-output, output] + */ + public void setOutputLimits(double output) { + setOutputLimits(-output, output); + } + + /** + * Specify a maximum output. When two inputs specified, output range is configured to + * [minimum, maximum] + * + * @param minimum possible output value + * @param maximum possible output value + */ + public void setOutputLimits(double minimum, double maximum) { + if (maximum < minimum) { + return; + } + maxOutput = maximum; + minOutput = minimum; + + // Ensure the bounds of the I term are within the bounds of the allowable output swing + if (maxIOutput == 0 || maxIOutput > (maximum - minimum)) { + setMaxIOutput(maximum - minimum); + } + } + + /** + * Set the operating direction of the PID controller + * + * @param reversed Set true to reverse PID output + */ + public void setDirection(boolean reversed) { + this.reversed = reversed; + } + + //********************************** + // Primary operating functions + //********************************** + + /** + * Configure setpoint for the PID calculations
This represents the target for the PID + * system's, such as a position, velocity, or angle.
+ */ + public void setSetpoint(double setpoint) { + this.setpoint = setpoint; + } + + /** + * Calculate the output value for the current PID cycle.
+ * + * @param actual The monitored value, typically as a sensor input. + * @param setpoint The target value for the system + * @return calculated output value for driving the system + */ + public double getOutput(double actual, double setpoint) { + double output; + double Poutput; + double Ioutput; + double Doutput; + double Foutput; + + this.setpoint = setpoint; + + // Ramp the setpoint used for calculations if user has opted to do so + if (setpointRange != 0) { + setpoint = constrain(setpoint, actual - setpointRange, actual + setpointRange); + } + + // Do the simple parts of the calculations + double error = setpoint - actual; + + // Calculate F output. Notice, this depends only on the setpoint, and not the error. + Foutput = F * setpoint; + + // Calculate P term + Poutput = P * error; + + // If this is our first time running this, we don't actually _have_ a previous input or output. + // For sensor, sanely assume it was exactly where it is now. + // For last output, we can assume it's the current time-independent outputs. + if (firstRun) { + lastActual = actual; + lastOutput = Poutput + Foutput; + firstRun = false; + } + + // Calculate D Term + // Note, this is negative. This actually "slows" the system if it's doing + // the correct thing, and small values helps prevent output spikes and overshoot + Doutput = -D * (actual - lastActual); + lastActual = actual; + + // The Iterm is more complex. There's several things to factor in to make it easier to deal with. + // 1. maxIoutput restricts the amount of output contributed by the Iterm. + // 2. prevent windup by not increasing errorSum if we're already running against our max Ioutput + // 3. prevent windup by not increasing errorSum if output is output=maxOutput + Ioutput = I * errorSum; + if (maxIOutput != 0) { + Ioutput = constrain(Ioutput, -maxIOutput, maxIOutput); + } + + // And, finally, we can just add the terms up + output = Foutput + Poutput + Ioutput + Doutput; + + // Figure out what we're doing with the error. + if (minOutput != maxOutput && !bounded(output, minOutput, maxOutput)) { + errorSum = error; + // reset the error sum to a sane level + // Setting to current error ensures a smooth transition when the P term + // decreases enough for the I term to start acting upon the controller + // From that point the I term will build up as would be expected + } else if (outputRampRate != 0 && !bounded(output, lastOutput - outputRampRate, + lastOutput + outputRampRate)) { + errorSum = error; + } else if (maxIOutput != 0) { + errorSum = constrain(errorSum + error, -maxError, maxError); + // In addition to output limiting directly, we also want to prevent I term + // buildup, so restrict the error directly + } else { + errorSum += error; + } + + // Restrict output to our specified output and ramp limits + if (outputRampRate != 0) { + output = constrain(output, lastOutput - outputRampRate, lastOutput + outputRampRate); + } + if (minOutput != maxOutput) { + output = constrain(output, minOutput, maxOutput); + } + if (outputFilter != 0) { + output = lastOutput * outputFilter + output * (1 - outputFilter); + } + + // Get a test printline with lots of details about the internal + // calculations. This can be useful for debugging. + // System.out.printf("Final output %5.2f [ %5.2f, %5.2f , %5.2f ], eSum %.2f\n",output,Poutput, Ioutput, Doutput,errorSum ); + // System.out.printf("%5.2f\t%5.2f\t%5.2f\t%5.2f\n",output,Poutput, Ioutput, Doutput ); + + lastOutput = output; + return output; + } + + /** + * Calculate the output value for the current PID cycle.
In no-parameter mode, this uses the + * last sensor value, and last setpoint value.
Not typically useful, and use of parameter + * modes is suggested.
+ * + * @return calculated output value for driving the system + */ + public double getOutput() { + return getOutput(lastActual, setpoint); + } + + /** + * Calculate the output value for the current PID cycle.
In one parameter mode, the last + * configured setpoint will be used.
+ * + * @param actual The monitored value, typically as a sensor input. + * @return calculated output value for driving the system + */ + public double getOutput(double actual) { + return getOutput(actual, setpoint); + } + + /** + * Resets the controller. This erases the I term buildup, and removes D gain on the next + * loop.
This should be used any time the PID is disabled or inactive for extended duration, + * and the controlled portion of the system may have changed due to external forces. + */ + public void reset() { + firstRun = true; + errorSum = 0; + } + + /** + * Set the maximum rate the output can increase per cycle.
This can prevent sharp jumps in + * output when changing setpoints or enabling a PID system, which might cause stress on physical + * or electrical systems.
Can be very useful for fast-reacting control loops, such as ones + * with large P or D values and feed-forward systems. + * + * @param rate, with units being the same as the output + */ + public void setOutputRampRate(double rate) { + outputRampRate = rate; + } + + /** + * Set a limit on how far the setpoint can be from the current position + *
Can simplify tuning by helping tuning over a small range applies to a much larger range. + *
This limits the reactivity of P term, and restricts impact of large D term + * during large setpoint adjustments. Increases lag and I term if range is too small. + * + * @param range, with units being the same as the expected sensor range. + */ + public void setSetpointRange(double range) { + setpointRange = range; + } + + /** + * Set a filter on the output to reduce sharp oscillations.
0.1 is likely a sane starting + * value. Larger values use historical data more heavily, with low values weigh newer data. 0 + * will disable, filtering, and use only the most recent value.
Increasing the filter + * strength will P and D oscillations, but force larger I values and increase I term + * overshoot.
Uses an exponential wieghted rolling sum filter, according to a simple
+ * 
output*(1-strength)*sum(0..n){output*strength^n}
algorithm. + */ + public void setOutputFilter(double strength) { + if (strength == 0 || bounded(strength, 0, 1)) { + outputFilter = strength; + } + } + + //************************************** + // Helper functions + //************************************** + + /** + * Forces a value into a specific range + * + * @param value input value + * @param min maximum returned value + * @param max minimum value in range + * @return Value if it's within provided range, min or max otherwise + */ + private double constrain(double value, double min, double max) { + if (value > max) { + return max; + } + if (value < min) { + return min; + } + return value; + } + + /** + * Test if the value is within the min and max, inclusive + * + * @param value to test + * @param min Minimum value of range + * @param max Maximum value of range + * @return true if value is within range, false otherwise + */ + private boolean bounded(double value, double min, double max) { + // Note, this is an inclusive range. This is so tests like + // `bounded(constrain(0,0,1),0,1)` will return false. + // This is more helpful for determining edge-case behaviour + // than <= is. + return (min < value) && (value < max); + } + + /** + * To operate correctly, all PID parameters require the same sign This should align with the + * {@literal}reversed value + */ + private void checkSigns() { + if (reversed) { // all values should be below zero + if (P > 0) { + P *= -1; + } + if (I > 0) { + I *= -1; + } + if (D > 0) { + D *= -1; + } + if (F > 0) { + F *= -1; + } + } else { // all values should be above zero + if (P < 0) { + P *= -1; + } + if (I < 0) { + I *= -1; + } + if (D < 0) { + D *= -1; + } + if (F < 0) { + F *= -1; + } + } + } +} diff --git a/src/main/java/org/team1540/rooster/util/SimpleAsyncCommand.java b/src/main/java/org/team1540/rooster/util/SimpleAsyncCommand.java deleted file mode 100644 index 8412021d..00000000 --- a/src/main/java/org/team1540/rooster/util/SimpleAsyncCommand.java +++ /dev/null @@ -1,53 +0,0 @@ -package org.team1540.rooster.util; - -import edu.wpi.first.wpilibj.Sendable; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import java.util.Objects; -import org.jetbrains.annotations.NotNull; -import org.team1540.rooster.functional.Executable; - -/** - * A simple way to construct an {@link AsyncCommand}.

To create a {@code SimpleCommand} easily, - * simply pass it a no-argument lambda containing the code you would like to execute. For example, - * to create a {@link AsyncCommand} that loops every 30 milliseconds and requires the {@code - * Robot.shifter} {@link Subsystem}, simply write: - * 

- *   Command shift = new SimpleAsyncCommand("Shift", 30, () -> Robot.shifter.shift(), Robot.shifter);
- *
- * Multiple {@code Subsystems} can be added onto the end of the constructor to add multiple - * requirements.

This can be used to quickly and easily put a button on the - * SmartDashboard/Shuffleboard to run some code. Use {@link SmartDashboard#putData(Sendable)} to - * pass it a newly created instance of this class. - * - * @see SimpleAsyncCommand - * @see SimpleLoopCommand - * @see Command - */ -public class SimpleAsyncCommand extends AsyncCommand { - - private final Executable executable; - - /** - * Constructs a new {@code SimpleAsyncCommand} with a preset periodic interval that runs the - * provided {@link Executable}. - * - * @param interval The interval between calls to the {@link Executable}, in milliseconds. - */ - public SimpleAsyncCommand(@NotNull String name, long interval, @NotNull Executable executable, - @NotNull Subsystem... requirements) { - super(interval); - this.setName(Objects.requireNonNull(name)); - - this.executable = Objects.requireNonNull(executable); - for (Subsystem requirement : requirements) { - requires(Objects.requireNonNull(requirement)); - } - } - - @Override - protected void runPeriodic() { - executable.execute(); - } -} diff --git a/src/main/java/org/team1540/rooster/util/SimpleCommand.java b/src/main/java/org/team1540/rooster/util/SimpleCommand.java deleted file mode 100644 index af6981ac..00000000 --- a/src/main/java/org/team1540/rooster/util/SimpleCommand.java +++ /dev/null @@ -1,52 +0,0 @@ -package org.team1540.rooster.util; - -import edu.wpi.first.wpilibj.Sendable; -import edu.wpi.first.wpilibj.command.InstantCommand; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import java.util.Objects; -import org.jetbrains.annotations.NotNull; -import org.team1540.rooster.functional.Executable; - -/** - * A simple way to construct an {@link InstantCommand}.

To create a {@code SimpleCommand} - * easily, simply pass it a no-argument lambda containing the code you would like to execute. For - * example, to create a {@link InstantCommand} that requires the {@code Robot.shifter} {@link - * Subsystem}, simply write: - * 

- *   Command shift = new SimpleCommand(() -> Robot.shifter.shift(), Robot.shifter);
- *
- * - * Multiple {@code Subystems} can be added onto the end of the constructor to add multiple - * requirements.

This can be used to quickly and easily put a button on the - * SmartDashboard/Shuffleboard to run some code. Use {@link SmartDashboard#putData(Sendable)} to - * pass it a newly created instance of this class. - */ -public class SimpleCommand extends InstantCommand { - - private Executable executable; - - /** - * Creates a new {@code SimpleCommand}. - * - * @param name The name of the commmand. This is required to avoid everything being called - * SimpleCommand. - * @param action The code to run when the command executes. - * @param requirements The {@link Subsystem Subsystems} required by the command (if any). - * @throws IllegalArgumentException If any parameters are {@code null}. - */ - public SimpleCommand(@NotNull String name, @NotNull Executable action, - @NotNull Subsystem... requirements) { - super(Objects.requireNonNull(name)); - executable = Objects.requireNonNull(action); - - for (Subsystem requirement : requirements) { - requires(Objects.requireNonNull(requirement)); - } - } - - @Override - protected void execute() { - executable.execute(); - } -} diff --git a/src/main/java/org/team1540/rooster/util/SimpleConditionalCommand.java b/src/main/java/org/team1540/rooster/util/SimpleConditionalCommand.java deleted file mode 100644 index 29358de2..00000000 --- a/src/main/java/org/team1540/rooster/util/SimpleConditionalCommand.java +++ /dev/null @@ -1,62 +0,0 @@ -package org.team1540.rooster.util; - -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.command.ConditionalCommand; -import java.util.Objects; -import java.util.function.BooleanSupplier; -import org.jetbrains.annotations.NotNull; -import org.jetbrains.annotations.Nullable; - - -/** - * A simple {@link ConditionalCommand} that can be constructed with a lambda method or function - * reference that supplies a {@code boolean}. - */ -public class SimpleConditionalCommand extends ConditionalCommand { - - private BooleanSupplier condition; - - /** - * Constructs a new {@code SimpleConditionalCommand} that uses the provided {@link - * BooleanSupplier} to determine whether to execute the provided {@link Command}. - * - * @param condition The {@code BooleanSupplier} to call for the condition. - * @param onTrue The command to execute when the supplier supplies a true value, or {@code null} - * if no command should be executed. - * @throws NullPointerException If {@code condition} is {@code null}. - */ - public SimpleConditionalCommand(@NotNull BooleanSupplier condition, @Nullable Command onTrue) { - this(condition, onTrue, null); - } - - /** - * Constructs a new {@code SimpleConditionalCommand} that uses the provided {@link - * BooleanSupplier} to determine which command to execute. - * - * @param condition The {@code BooleanSupplier} to call for the condition. - * @param onTrue The command to execute when the supplier supplies a true value, or {@code null} * - * if no command should be executed. - * @param onFalse The command to execute when the supplier supplies a false value, or {@code null} - * if no command should be executed. - * @throws NullPointerException If {@code condition} is {@code null}. - */ - public SimpleConditionalCommand(@NotNull BooleanSupplier condition, @Nullable Command onTrue, - @Nullable Command onFalse) { - super(onTrue, onFalse); - this.condition = Objects.requireNonNull(condition); - } - - @Override - public boolean condition() { - return condition.getAsBoolean(); - } - - @NotNull - public BooleanSupplier getCondition() { - return condition; - } - - public void setCondition(@NotNull BooleanSupplier condition) { - this.condition = condition; - } -} diff --git a/src/main/java/org/team1540/rooster/util/SimpleLoopCommand.java b/src/main/java/org/team1540/rooster/util/SimpleLoopCommand.java deleted file mode 100644 index a236015e..00000000 --- a/src/main/java/org/team1540/rooster/util/SimpleLoopCommand.java +++ /dev/null @@ -1,57 +0,0 @@ -package org.team1540.rooster.util; - -import edu.wpi.first.wpilibj.Sendable; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import java.util.Objects; -import org.jetbrains.annotations.NotNull; -import org.team1540.rooster.functional.Executable; - -/** - * A simple way to construct a {@link Command} which executes every tick.

To create a {@code - * SimpleLoopCommand} easily, simply pass it a no-argument lambda containing the code you would like - * to execute. For example, to create a {@link Command} that requires the {@code Robot.shifter} - * {@link Subsystem}, simply write: - * 

- *   Command shift = new SimpleLoopCommand(() -> Robot.shifter.shift(), Robot.shifter);
- *
- * - * Multiple {@code Subystems} can be added onto the end of the constructor to add multiple - * requirements.

This can be used to quickly and easily put a button on the - * SmartDashboard/Shuffleboard to run some code. Use {@link SmartDashboard#putData(Sendable)} to - * pass it a newly created instance of this class. - */ -public class SimpleLoopCommand extends Command { - - private Executable executable; - - /** - * Creates a new {@code SimpleLoopCommand}. - * - * @param name The name of the commmand. This is required to avoid everything being called - * SimpleCommand. - * @param action The code to run when the command executes. - * @param requirements The {@link Subsystem Subsystems} required by the command (if any). - * @throws IllegalArgumentException If any parameters are {@code null}. - */ - public SimpleLoopCommand(@NotNull String name, @NotNull Executable action, - @NotNull Subsystem... requirements) { - super(Objects.requireNonNull(name)); - executable = Objects.requireNonNull(action); - - for (Subsystem requirement : requirements) { - requires(Objects.requireNonNull(requirement)); - } - } - - @Override - protected void execute() { - executable.execute(); - } - - @Override - protected boolean isFinished() { - return false; - } -} diff --git a/src/main/java/org/team1540/rooster/util/StateChangeDetector.java b/src/main/java/org/team1540/rooster/util/StateChangeDetector.java new file mode 100644 index 00000000..f97a811e --- /dev/null +++ b/src/main/java/org/team1540/rooster/util/StateChangeDetector.java @@ -0,0 +1,38 @@ +package org.team1540.rooster.util; + +public class StateChangeDetector { + + public boolean oldState; + + public StateChangeDetector(boolean startState) { + this.oldState = startState; + } + + public boolean getOldState() { + return oldState; + } + + public boolean didChange(boolean currentState) { + if (currentState != oldState) { + oldState = currentState; + return true; + } + return false; + } + + public boolean didChangeToTrue(boolean currentState) { + if (!oldState && currentState) { + oldState = true; + return true; + } + return false; + } + + public boolean didChangeToFalse(boolean currentState) { + if (oldState && !currentState) { + oldState = false; + return true; + } + return false; + } +} diff --git a/src/main/java/org/team1540/rooster/util/StickyFaultsUtils.java b/src/main/java/org/team1540/rooster/util/StickyFaultsUtils.java new file mode 100644 index 00000000..1989ed28 --- /dev/null +++ b/src/main/java/org/team1540/rooster/util/StickyFaultsUtils.java @@ -0,0 +1,48 @@ +package org.team1540.rooster.util; + +import com.ctre.phoenix.motorcontrol.StickyFaults; +import edu.wpi.first.wpilibj.shuffleboard.EventImportance; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import org.apache.log4j.Logger; +import org.jetbrains.annotations.NotNull; +import org.team1540.rooster.wrappers.ChickenController; + +public class StickyFaultsUtils { + + private static final Logger logger = Logger.getLogger(StickyFaultsUtils.class); + + public static void processStickyFaults(String subsystemName, String motorName, + @NotNull ChickenController controller) { + var stickyFaults = new StickyFaults(); + controller.getStickyFaults(stickyFaults); + if (stickyFaults.hasAnyFault()) { + var stringBuilder = new StringBuilder() + .append(stickyFaults.ForwardLimitSwitch ? "fwd lim switch, " : "") + .append(stickyFaults.ReverseLimitSwitch ? "rev lim switch, " : "") + .append(stickyFaults.ForwardSoftLimit ? "fwd soft limit, " : "") + .append(stickyFaults.ReverseSoftLimit ? "rev soft limit, " : "") + .append(stickyFaults.HardwareESDReset ? "hardware ESD reset, " : "") + .append(stickyFaults.RemoteLossOfSignal ? "remote loss of signal, " : "") + .append(stickyFaults.ResetDuringEn ? "reset during enable, " : "") + .append(stickyFaults.SensorOutOfPhase ? "sensor out of phase, " : "") + .append(stickyFaults.SensorOverflow ? "sensor overflow, " : "") + .append(stickyFaults.UnderVoltage ? "undervoltage, " : ""); + + if (stringBuilder.length() != 0) { + // delete the last comma + stringBuilder.delete(stringBuilder.length() - 3, stringBuilder.length()); + } + + String description = + subsystemName + " controller " + controller.getDeviceID() + "(" + motorName + + ") had sticky faults: " + stringBuilder.toString(); + + logger.warn(description); + + Shuffleboard + .addEventMarker(subsystemName + " sticky fault", description, + EventImportance.kHigh); + } + } + +} diff --git a/src/main/java/org/team1540/rooster/util/TrigUtils.java b/src/main/java/org/team1540/rooster/util/TrigUtils.java new file mode 100644 index 00000000..d1126630 --- /dev/null +++ b/src/main/java/org/team1540/rooster/util/TrigUtils.java @@ -0,0 +1,13 @@ +package org.team1540.rooster.util; + +public class TrigUtils { + + public static double signedAngleError(double target, double source) { + double diff = (target - source + Math.PI) % (Math.PI * 2) - Math.PI; + return diff < -Math.PI ? diff + (Math.PI * 2) : diff; + } + + public static double radiusFromArcAndAngle(double arcLength, double centralAngle) { + return arcLength / centralAngle; + } +} diff --git a/src/main/java/org/team1540/rooster/util/package-info.java b/src/main/java/org/team1540/rooster/util/package-info.java index f5e7cedf..a97bccd0 100644 --- a/src/main/java/org/team1540/rooster/util/package-info.java +++ b/src/main/java/org/team1540/rooster/util/package-info.java @@ -1,7 +1,5 @@ /** - * Utility classes, mostly consisting of extensions to the WPILib {@link - * edu.wpi.first.wpilibj.command.Command} framework, to make life easier. Also includes the {@link - * org.team1540.rooster.util.robots robots} subpackage with self-contained utility and testing - * robots. + * Various utilities to make life easier. Also includes the {@link org.team1540.rooster.util.robots + * robots} subpackage with self-contained utility and testing robots. */ package org.team1540.rooster.util; diff --git a/src/main/java/org/team1540/rooster/util/robots/AccelerationCharacterizationRobot.java b/src/main/java/org/team1540/rooster/util/robots/AccelerationCharacterizationRobot.java deleted file mode 100644 index 7e032da0..00000000 --- a/src/main/java/org/team1540/rooster/util/robots/AccelerationCharacterizationRobot.java +++ /dev/null @@ -1,233 +0,0 @@ -package org.team1540.rooster.util.robots; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import edu.wpi.first.wpilibj.IterativeRobot; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.command.Scheduler; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import java.io.File; -import java.io.FileNotFoundException; -import java.io.PrintWriter; -import java.util.LinkedList; -import java.util.List; -import org.apache.commons.math3.stat.regression.SimpleRegression; -import org.team1540.rooster.preferencemanager.Preference; -import org.team1540.rooster.preferencemanager.PreferenceManager; -import org.team1540.rooster.wrappers.ChickenTalon; - -/** - * Self-contained robot class to characterize a drivetrain's acceleration term. - * - * When deployed to a three-motor-per-side robot with left motors on motors 1, 2, and 3 and right - * motors on motors 4, 5, and 6, whenever the A button is pressed and held during teleop the robot - * will carry out an acceleration characterization as described in Eli Barnett's paper "FRC - * Drivetrain Characterization" until the button is released. (Encoders should be on motors 1 and - * 4.) The results will be saved in a file in the /home/lvuser/dtmeasure directory named - * "measureaccel-" followed by the Unix timestamp of the test start and ".csv". - */ -public class AccelerationCharacterizationRobot extends IterativeRobot { - - @Preference("kV") - public double kV; - @Preference("VIntercept") - public double vIntercept; - - @Preference("Invert Left Motor") - public boolean invertLeft = false; - @Preference("Invert Right Motor") - public boolean invertRight = true; - @Preference(persistent = false) - public int accelMeasurementWindow = 6; - - @Preference(persistent = false) - public double setpoint = 0.6; - - private ChickenTalon driveLeftMotorA = new ChickenTalon(1); - private ChickenTalon driveLeftMotorB = new ChickenTalon(2); - private ChickenTalon driveLeftMotorC = new ChickenTalon(3); - private ChickenTalon[] driveLeftMotors = new ChickenTalon[]{driveLeftMotorA, driveLeftMotorB, - driveLeftMotorC}; - private ChickenTalon driveRightMotorA = new ChickenTalon(4); - private ChickenTalon driveRightMotorB = new ChickenTalon(5); - private ChickenTalon driveRightMotorC = new ChickenTalon(6); - private ChickenTalon[] driveRightMotors = new ChickenTalon[]{driveRightMotorA, driveRightMotorB, - driveRightMotorC}; - private ChickenTalon[] driveMotorAll = new ChickenTalon[]{driveLeftMotorA, driveLeftMotorB, - driveLeftMotorC, driveRightMotorA, driveRightMotorB, driveRightMotorC}; - private ChickenTalon[] driveMotorMasters = new ChickenTalon[]{driveLeftMotorA, driveRightMotorA}; - - private PrintWriter csvWriter = null; - - private Notifier notifier = new Notifier(this::run); - - @SuppressWarnings("SuspiciousNameCombination") - private void run() { - if (!isOperatorControl() && csvWriter != null) { - csvWriter.close(); - csvWriter = null; - } - if (leftVelocities.size() == accelMeasurementWindow) { - leftVelocities.remove(0); - rightVelocities.remove(0); - leftVoltages.remove(0); - rightVoltages.remove(0); - times.remove(0); - } - double leftVelocity = driveLeftMotorA.getSelectedSensorVelocity(); - double rightVelocity = driveRightMotorA.getSelectedSensorVelocity(); - - leftVelocities.add(leftVelocity); - rightVelocities.add(rightVelocity); - - double accelCausingVoltageLeft = - driveLeftMotorA.getMotorOutputVoltage() - (kV * leftVelocity - + vIntercept); - double accelCausingVoltageRight = - driveRightMotorA.getMotorOutputVoltage() - (kV * rightVelocity - + vIntercept); - leftVoltages.add(accelCausingVoltageLeft); - rightVoltages.add(accelCausingVoltageRight); - times.add((double) System.currentTimeMillis() / 1000.0); - - if (leftVelocities.size() == accelMeasurementWindow) { - double lAccel = bestFitSlope(times, leftVelocities); - double rAccel = bestFitSlope(times, rightVelocities); - SmartDashboard.putNumber("Left Accel", lAccel); - SmartDashboard.putNumber("Right Accel", rAccel); - } - - if (joystick.getRawButton(1)) { // if button A is pressed - if (csvWriter == null) { - // create a new CSV writer, reset everything - reset(); - try { - File dir = new File("/home/lvuser/dtmeasure/"); - if (!dir.exists()) { - dir.mkdirs(); - } - csvWriter = new PrintWriter(new File( - "/home/lvuser/dtmeasure/measureaccel-" + System.currentTimeMillis() + ".csv")); - csvWriter.println("lvoltage,laccel,rvoltage,raccel"); - } catch (FileNotFoundException e) { - throw new RuntimeException(e); - } - driveLeftMotorA.set(ControlMode.PercentOutput, 0); - driveRightMotorA.set(ControlMode.PercentOutput, 0); - } else { - SmartDashboard.putNumber("Left Output", driveLeftMotorA.getMotorOutputPercent()); - SmartDashboard.putNumber("Left Velocity", leftVelocity); - SmartDashboard.putNumber("Right Output", driveRightMotorA.getMotorOutputPercent()); - SmartDashboard.putNumber("Right Velocity", rightVelocity); - - if (leftVelocities.size() == accelMeasurementWindow) { - double lAccel = bestFitSlope(times, leftVelocities); - double rAccel = bestFitSlope(times, rightVelocities); - csvWriter.println( - leftVoltages.get(1) + "," + lAccel + "," + rightVoltages.get(1) + "," + rAccel); - System.out.println(leftVelocities.toString()); - System.out.println(times.toString()); - System.out.println(lAccel); - } - driveLeftMotorA.set(ControlMode.PercentOutput, setpoint); - driveRightMotorA.set(ControlMode.PercentOutput, setpoint); - } - } else { - if (csvWriter != null) { - csvWriter.close(); - csvWriter = null; - } - driveLeftMotorA.set(ControlMode.PercentOutput, 0); - driveRightMotorA.set(ControlMode.PercentOutput, 0); - } - } - - private Joystick joystick = new Joystick(0); - - private List leftVelocities = new LinkedList<>(); - private List leftVoltages = new LinkedList<>(); - private List rightVelocities = new LinkedList<>(); - private List rightVoltages = new LinkedList<>(); - private List times = new LinkedList<>(); - - @Override - public void robotInit() { - PreferenceManager.getInstance().add(this); - reset(); - notifier.startPeriodic(0.01); - - SmartDashboard.setDefaultNumber("Setpoint", 0.6); - } - - @Override - public void robotPeriodic() { - Scheduler.getInstance().run(); // process preferences - } - - @Override - public void teleopPeriodic() { - - } - - @Override - public void teleopInit() { - reset(); - for (ChickenTalon talon : driveMotorAll) { - talon.setBrake(true); - } - } - - @Override - public void disabledInit() { - for (ChickenTalon talon : driveMotorAll) { - talon.setBrake(false); - } - } - - @SuppressWarnings("Duplicates") - public void reset() { - driveLeftMotorA.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); - driveRightMotorA.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); - - driveLeftMotorA.setSensorPhase(true); - - for (ChickenTalon talon : driveLeftMotors) { - talon.setInverted(invertLeft); - } - - driveRightMotorA.setSensorPhase(true); - - for (ChickenTalon talon : driveRightMotors) { - talon.setInverted(invertRight); - } - - driveLeftMotorB.set(ControlMode.Follower, driveLeftMotorA.getDeviceID()); - driveLeftMotorC.set(ControlMode.Follower, driveLeftMotorA.getDeviceID()); - - driveRightMotorB.set(ControlMode.Follower, driveRightMotorA.getDeviceID()); - driveRightMotorC.set(ControlMode.Follower, driveRightMotorA.getDeviceID()); - - for (ChickenTalon talon : driveMotorAll) { - talon.setBrake(true); - } - - for (ChickenTalon talon : driveMotorAll) { - talon.configClosedloopRamp(0); - talon.configOpenloopRamp(0); - talon.configPeakOutputForward(1); - talon.configPeakOutputReverse(-1); - talon.enableCurrentLimit(false); - talon.configVoltageCompSaturation(12); - talon.enableVoltageCompensation(true); - } - } - - private static double bestFitSlope(List xVals, List yVals) { - SimpleRegression reg = new SimpleRegression(); - for (int i = 0; i < xVals.size(); i++) { - reg.addData(xVals.get(i), yVals.get(i)); - } - return reg.getSlope(); - } -} diff --git a/src/main/java/org/team1540/rooster/util/robots/PIDTuningRobot.java b/src/main/java/org/team1540/rooster/util/robots/PIDTuningRobot.java index d69fab70..8eb220f7 100644 --- a/src/main/java/org/team1540/rooster/util/robots/PIDTuningRobot.java +++ b/src/main/java/org/team1540/rooster/util/robots/PIDTuningRobot.java @@ -3,16 +3,15 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import edu.wpi.first.wpilibj.IterativeRobot; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.command.Scheduler; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.InstantCommand; import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.Nullable; -import org.team1540.rooster.Utilities; import org.team1540.rooster.preferencemanager.Preference; import org.team1540.rooster.preferencemanager.PreferenceManager; -import org.team1540.rooster.util.SimpleCommand; +import org.team1540.rooster.util.ControlUtils; import org.team1540.rooster.wrappers.ChickenTalon; /** @@ -80,14 +79,14 @@ public void robotInit() { + " * allow the values to take effect. To disable a motor, set its motor ID to -1. Motor 1 will be \n" + " * configured as the master Talon and motors 2, 3, and 4 will be slaved to it in follower mode."); PreferenceManager.getInstance().add(this); - Scheduler.getInstance().run(); // allow the PreferenceManager to update + CommandScheduler.getInstance().run(); // allow the PreferenceManager to update - controlModeChooser.addDefault("Position", ControlMode.Position); - controlModeChooser.addDefault("Velocity", ControlMode.Velocity); - controlModeChooser.addDefault("MotionMagic", ControlMode.MotionMagic); + controlModeChooser.setDefaultOption("Position", ControlMode.Position); + controlModeChooser.addOption("Velocity", ControlMode.Velocity); + controlModeChooser.addOption("MotionMagic", ControlMode.MotionMagic); SmartDashboard.putData("Control Mode Chooser", controlModeChooser); - Command reset = new SimpleCommand("Reset", () -> { + var reset = new InstantCommand(() -> { if (motor1ID != -1) { motor1 = new ChickenTalon(motor1ID); } else { @@ -116,17 +115,25 @@ public void robotInit() { motor.enableCurrentLimit(false); } } - }); - reset.setRunWhenDisabled(true); - reset.start(); + }) { + @Override + public boolean runsWhenDisabled() { + return true; + } + }; + reset.schedule(); SmartDashboard.putData(reset); - Command zero = new SimpleCommand("Zero Position", () -> { + var zero = new InstantCommand(() -> { if (motor1 != null) { motor1.setSelectedSensorPosition(0); } - }); - zero.setRunWhenDisabled(true); + }) { + @Override + public boolean runsWhenDisabled() { + return true; + } + }; SmartDashboard.putData(zero); } @@ -144,15 +151,14 @@ public void teleopPeriodic() { if (enablePID) { motor1.set(controlModeChooser.getSelected(), setpoint); } else { - motor1 - .set(ControlMode.PercentOutput, Utilities.processDeadzone(joystick.getRawAxis(1), 0.1)); + motor1.set(ControlMode.PercentOutput, ControlUtils.deadzone(joystick.getRawAxis(1), 0.1)); } } } @Override public void robotPeriodic() { - Scheduler.getInstance().run(); + CommandScheduler.getInstance().run(); for (ChickenTalon motor : new ChickenTalon[]{motor1, motor2, motor3, motor4}) { if (motor != null) { motor.setInverted(invertOutput); diff --git a/src/main/java/org/team1540/rooster/util/robots/VelocityCharacterizationRobot.java b/src/main/java/org/team1540/rooster/util/robots/VelocityCharacterizationRobot.java deleted file mode 100644 index 3ed95fdc..00000000 --- a/src/main/java/org/team1540/rooster/util/robots/VelocityCharacterizationRobot.java +++ /dev/null @@ -1,240 +0,0 @@ -package org.team1540.rooster.util.robots; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import edu.wpi.first.wpilibj.IterativeRobot; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.command.Scheduler; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import org.apache.commons.math3.stat.regression.SimpleRegression; -import org.jetbrains.annotations.NotNull; -import org.team1540.rooster.preferencemanager.Preference; -import org.team1540.rooster.preferencemanager.PreferenceManager; -import org.team1540.rooster.util.SimpleCommand; -import org.team1540.rooster.wrappers.ChickenTalon; - -//TODO MORE DOCS - -/** - * Self-contained robot class to characterize a drivetrain's velocity term. - * - * Whenever the A button is pressed and held during teleop the robot will carry out a quasi-static - * velocity characterization as described in Eli Barnett's paper "FRC Drivetrain Characterization" - * until the button is released. The results (kV, vIntercept, and an R^2 value) are output to the - * SmartDashboard. - */ -public class VelocityCharacterizationRobot extends IterativeRobot { - - @Preference(persistent = false) - public int lMotor1ID = -1; - @Preference(persistent = false) - public int lMotor2ID = -1; - @Preference(persistent = false) - public int lMotor3ID = -1; - @Preference(persistent = false) - public int rMotor1ID = -1; - @Preference(persistent = false) - public int rMotor2ID = -1; - @Preference(persistent = false) - public int rMotor3ID = -1; - @Preference(persistent = false) - public boolean invertLeftMotor = false; - @Preference(persistent = false) - public boolean invertRightMotor = false; - @Preference(persistent = false) - public boolean invertLeftSensor = false; - @Preference(persistent = false) - public boolean invertRightSensor = false; - @Preference(persistent = false) - public boolean brake = false; - @Preference(persistent = false) - public double tpu = 1; - - - private ChickenTalon driveLeftMotorA; - private ChickenTalon driveLeftMotorB; - private ChickenTalon driveLeftMotorC; - private ChickenTalon driveRightMotorA; - private ChickenTalon driveRightMotorB; - private ChickenTalon driveRightMotorC; - - private SimpleRegression leftRegression = new SimpleRegression(); - private SimpleRegression rightRegression = new SimpleRegression(); - private boolean running = false; - - private Joystick joystick = new Joystick(0); - - private static final double SATURATION_VOLTAGE = 12; - @Preference("Voltage Ramp Rate (V/sec)") - public double rampRate = 0.25; - - @Preference("Invert Left Motor") - public boolean invertLeft = false; - @Preference("Invert Right Motor") - public boolean invertRight = true; - - private double appliedOutput = 0; - - public long lastTime; - - @Override - public void robotInit() { - PreferenceManager.getInstance().add(this); - Command reset = new SimpleCommand("Reset", () -> { - if (lMotor1ID != -1) { - driveLeftMotorA = new ChickenTalon(lMotor1ID); - } else { - System.err.println("Left Motor 1 must be set!"); - return; - } - if (lMotor2ID != -1) { - driveLeftMotorB = new ChickenTalon(lMotor2ID); - driveLeftMotorB.set(ControlMode.Follower, driveLeftMotorA.getDeviceID()); - } else { - if (driveLeftMotorB != null) { - driveLeftMotorB.set(ControlMode.PercentOutput, 0); - } - driveLeftMotorB = null; - } - if (lMotor3ID != -1) { - driveLeftMotorC = new ChickenTalon(lMotor3ID); - driveLeftMotorC.set(ControlMode.Follower, driveLeftMotorA.getDeviceID()); - } else { - if (driveLeftMotorC != null) { - driveLeftMotorC.set(ControlMode.PercentOutput, 0); - } - driveLeftMotorC = null; - } - - if (rMotor1ID != -1) { - driveRightMotorA = new ChickenTalon(rMotor1ID); - } else { - System.err.println("Right Motor 1 must be set!"); - return; - } - if (rMotor2ID != -1) { - driveRightMotorB = new ChickenTalon(rMotor2ID); - driveRightMotorB.set(ControlMode.Follower, driveRightMotorA.getDeviceID()); - } else { - if (driveRightMotorB != null) { - driveRightMotorB.set(ControlMode.PercentOutput, 0); - } - driveRightMotorB = null; - } - if (rMotor3ID != -1) { - driveRightMotorC = new ChickenTalon(rMotor3ID); - driveRightMotorC.set(ControlMode.Follower, driveRightMotorA.getDeviceID()); - } else { - if (driveRightMotorC != null) { - driveRightMotorC.set(ControlMode.PercentOutput, 0); - } - driveRightMotorC = null; - } - for (ChickenTalon motor : new ChickenTalon[]{driveLeftMotorA, driveLeftMotorB, - driveLeftMotorC, driveRightMotorA, driveRightMotorB, - driveRightMotorC}) { - if (motor != null) { - motor.configVoltageCompSaturation(SATURATION_VOLTAGE); - motor.enableVoltageCompensation(true); - motor.configClosedloopRamp(0); - motor.configOpenloopRamp(0); - motor.configPeakOutputForward(1); - motor.configPeakOutputReverse(-1); - motor.enableCurrentLimit(false); - motor.setBrake(brake); - } - } - }); - reset.setRunWhenDisabled(true); - reset.start(); - SmartDashboard.putData(reset); - - Command zero = new SimpleCommand("Zero", () -> { - if (driveLeftMotorA != null) { - driveLeftMotorA.setSelectedSensorPosition(0); - } - - if (driveRightMotorA != null) { - driveRightMotorA.setSelectedSensorPosition(0); - } - }); - zero.setRunWhenDisabled(true); - SmartDashboard.putData(zero); - } - - private static void putRegressionData(@NotNull SimpleRegression regression, String prefix) { - // getSlope, getIntercept, and getRSquare all have the same criteria for returning NaN - if (!Double.isNaN(regression.getSlope())) { - SmartDashboard.putNumber(prefix + " Calculated kV", regression.getSlope()); - SmartDashboard.putNumber(prefix + " Calculated vIntercept", regression.getIntercept()); - SmartDashboard.putNumber(prefix + " rSquared", regression.getRSquare()); - } else { - SmartDashboard.putNumber(prefix + " Calculated kV", 0); - SmartDashboard.putNumber(prefix + " Calculated vIntercept", 0); - SmartDashboard.putNumber(prefix + " rSquared", 0); - } - } - - @Override - public void robotPeriodic() { - putRegressionData(leftRegression, "Left"); - putRegressionData(rightRegression, "Right"); - Scheduler.getInstance().run(); - if (driveLeftMotorA != null) { - driveLeftMotorA.setSensorPhase(invertLeftSensor); - } - for (ChickenTalon talon : new ChickenTalon[]{driveLeftMotorA, driveLeftMotorB, - driveLeftMotorC}) { - if (talon != null) { - talon.setInverted(invertLeftMotor); - } - } - - if (driveRightMotorA != null) { - driveRightMotorA.setSensorPhase(invertRightSensor); - } - for (ChickenTalon talon : new ChickenTalon[]{driveRightMotorA, driveRightMotorB, - driveRightMotorC}) { - if (talon != null) { - talon.setInverted(invertRightMotor); - } - } - } - - @Override - public void teleopPeriodic() { - if (joystick.getRawButton(1)) { // if button A is pressed - if (!running) { - // reset everything - running = true; - leftRegression.clear(); - rightRegression.clear(); - appliedOutput = 0; - driveLeftMotorA.set(ControlMode.PercentOutput, 0); - driveRightMotorA.set(ControlMode.PercentOutput, 0); - } else { - double leftVelocity = (driveLeftMotorA.getSelectedSensorVelocity() * 10) / tpu; - if (leftVelocity != 0) { - leftRegression.addData(leftVelocity, driveLeftMotorA.getMotorOutputVoltage()); - } - - double rightVelocity = (driveRightMotorA.getSelectedSensorVelocity() * 10) / tpu; - if (rightVelocity != 0) { - rightRegression.addData(leftVelocity, driveRightMotorA.getMotorOutputVoltage()); - } - - appliedOutput += - (rampRate / SATURATION_VOLTAGE) * ((System.currentTimeMillis() - lastTime) / 1000.0); - lastTime = System.currentTimeMillis(); - driveLeftMotorA.set(ControlMode.PercentOutput, appliedOutput); - driveRightMotorA.set(ControlMode.PercentOutput, appliedOutput); - } - } else { - appliedOutput = 0; - driveLeftMotorA.set(ControlMode.PercentOutput, 0); - driveRightMotorA.set(ControlMode.PercentOutput, 0); - running = false; - } - lastTime = System.currentTimeMillis(); - } -} diff --git a/src/main/java/org/team1540/rooster/util/robots/WheelbaseTestRobot.java b/src/main/java/org/team1540/rooster/util/robots/WheelbaseTestRobot.java index 484d7bec..e7fd6328 100644 --- a/src/main/java/org/team1540/rooster/util/robots/WheelbaseTestRobot.java +++ b/src/main/java/org/team1540/rooster/util/robots/WheelbaseTestRobot.java @@ -3,12 +3,11 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import edu.wpi.first.wpilibj.IterativeRobot; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.command.Scheduler; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.InstantCommand; import org.team1540.rooster.preferencemanager.Preference; import org.team1540.rooster.preferencemanager.PreferenceManager; -import org.team1540.rooster.util.SimpleCommand; import org.team1540.rooster.wrappers.ChickenTalon; /** @@ -65,7 +64,7 @@ public void robotInit() { PreferenceManager.getInstance().add(this); - Command reset = new SimpleCommand("Reset", () -> { + var reset = new InstantCommand(() -> { if (lMotor1ID != -1) { lMotor1 = new ChickenTalon(lMotor1ID); lMotor1.setSensorPhase(invertLeftSensor); @@ -135,12 +134,17 @@ public void robotInit() { for (ChickenTalon motor : new ChickenTalon[]{rMotor1, rMotor2, rMotor3}) { motor.setInverted(invertRightMotor); } - }); - reset.setRunWhenDisabled(true); - reset.start(); - SmartDashboard.putData(reset); + }) { + @Override + public boolean runsWhenDisabled() { + return true; + } + }; + reset.setName("Reset"); + reset.schedule(); + SmartDashboard.putData("Reset", reset); - Command zero = new SimpleCommand("Zero", () -> { + var zero = new InstantCommand(() -> { if (lMotor1 != null) { lMotor1.setSelectedSensorPosition(0); } @@ -148,9 +152,14 @@ public void robotInit() { if (rMotor1 != null) { rMotor1.setSelectedSensorPosition(0); } - }); - zero.setRunWhenDisabled(true); - SmartDashboard.putData(zero); + }) { + @Override + public boolean runsWhenDisabled() { + return true; + } + }; + zero.setName("Zero"); + SmartDashboard.putData("Zero", zero); } @Override @@ -171,7 +180,7 @@ public void teleopPeriodic() { @Override public void robotPeriodic() { - Scheduler.getInstance().run(); + CommandScheduler.getInstance().run(); if (lMotor1 != null && rMotor1 != null) { SmartDashboard.putNumber("LPOS", lMotor1.getSelectedSensorPosition()); SmartDashboard.putNumber("RPOS", rMotor1.getSelectedSensorPosition()); diff --git a/src/main/java/org/team1540/rooster/wrappers/ChickenTalon.java b/src/main/java/org/team1540/rooster/wrappers/ChickenTalon.java index b4321c38..f5dd5aff 100644 --- a/src/main/java/org/team1540/rooster/wrappers/ChickenTalon.java +++ b/src/main/java/org/team1540/rooster/wrappers/ChickenTalon.java @@ -751,26 +751,6 @@ public int getStatusFramePeriod(StatusFrame frame) { return super.getStatusFramePeriod(frame, defaultTimeoutMs); } - /** - * Is forward limit switch closed. - * - * @return '1' iff forward limit switch is closed, 0 iff switch is open. This function works - * regardless if limit switch feature is enabled. - */ - public boolean isFwdLimitSwitchClosed() { - return getSensorCollection().isFwdLimitSwitchClosed(); - } - - /** - * Is reverse limit switch closed. - * - * @return '1' iff reverse limit switch is closed, 0 iff switch is open. This function works - * regardless if limit switch feature is enabled. - */ - public boolean isRevLimitSwitchClosed() { - return getSensorCollection().isRevLimitSwitchClosed(); - } - /** * Selects which profile slot to use for closed-loop control. * diff --git a/src/main/java/org/team1540/rooster/wrappers/Limelight.java b/src/main/java/org/team1540/rooster/wrappers/Limelight.java new file mode 100644 index 00000000..4509bd20 --- /dev/null +++ b/src/main/java/org/team1540/rooster/wrappers/Limelight.java @@ -0,0 +1,108 @@ +package org.team1540.rooster.wrappers; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import java.util.ArrayList; +import java.util.List; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; + +public class Limelight { + + private static final double HORIZONTAL_FOV = Math.toRadians(59.6); + private static final double VERTICAL_FOV = Math.toRadians(45.7); + private static final Vector2D CAM_RESOLUTION = new Vector2D(320, 240); + + private final NetworkTable limelightTable; + + /** + * Constructs a new limelight interface with the default hostname. + * + * @param name hostname of the limelight + */ + public Limelight(String name) { + limelightTable = NetworkTableInstance.getDefault().getTable(name); + } + + public NetworkTable getNetworkTable() { + return limelightTable; + } + + public double getHorizontalFov() { + return HORIZONTAL_FOV; + } + + public double getVerticalFov() { + return VERTICAL_FOV; + } + + public Vector2D getResolution() { return CAM_RESOLUTION; } + /** + * Gets the output of the limelight targeting from the network table. + * + * @return a {@link Vector2D} containing the output angles of the limelight targeting in radians + */ + public Vector2D getTargetAngles() { // TODO: This should be negated appropriately + double x = Math.toRadians(limelightTable.getEntry("tx").getDouble(0)); + double y = Math.toRadians(limelightTable.getEntry("ty").getDouble(0)); + return new Vector2D(x, y); + } + + /** + * Queries whether the limelight target has been found. + * + * @return the state of the target + */ + public boolean isTargetFound() { + return (double) limelightTable.getEntry("tv").getNumber(0) > 0; + } + + + /** + * Sets limelight's green LEDs on or off. + * + * @param isOn the new state of the LEDs + */ + public void setLeds(boolean isOn) { + if (getLeds() != isOn) { + limelightTable.getEntry("ledMode").setNumber(isOn ? 0 : 1); + NetworkTableInstance.getDefault().flush(); + } + } + + public boolean getLeds() { + return limelightTable.getEntry("ledMode").getDouble(1) == 0; + } + + /** + * Sets limelight to driver cam or vision mode. + * + * @param driverCam Whether the limelight should be in driver cam mode + */ + public void setDriverCam(boolean driverCam) { + limelightTable.getEntry("camMode").setNumber(driverCam ? 1 : 0); + NetworkTableInstance.getDefault().flush(); + } + + public void setPipeline(double id) { + if (getPipeline() != id) { + limelightTable.getEntry("pipeline").setNumber(id); + NetworkTableInstance.getDefault().flush(); + } + } + + public long getPipeline() { + return Math.round((double) limelightTable.getEntry("getpipe").getNumber(-1)); + } + + + public List getCorners() { + Double[] xCorners = limelightTable.getEntry("tcornx").getDoubleArray(new Double[]{}); + Double[] yCorners = limelightTable.getEntry("tcorny").getDoubleArray(new Double[]{}); + List cornerList = new ArrayList<>(); + for (int i = 0; i < xCorners.length; i++) { + cornerList.add(new Vector2D(xCorners[i], yCorners[i])); + } + return cornerList; + } + +} diff --git a/src/main/java/org/team1540/rooster/wrappers/NavX.java b/src/main/java/org/team1540/rooster/wrappers/NavX.java new file mode 100644 index 00000000..2fc2ad01 --- /dev/null +++ b/src/main/java/org/team1540/rooster/wrappers/NavX.java @@ -0,0 +1,46 @@ +package org.team1540.rooster.wrappers; + +import com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.wpilibj.SPI.Port; + +public class NavX { + + private final AHRS navx; + + public NavX(Port port) { + navx = new AHRS(port); + } + + /** + * @return NavX yaw counter-clockwise in radians, from -pi to pi. This method does NOT continue + * past pi or -pi and is thus the one you probably want to use most of the time. + */ + public double getYawRadians() { + return -Math.toRadians(navx.getYaw()); + } + + /** + * @return NavX angle counter-clockwise in radians. This method continues past pi and -pi and is + * thus the one you don't want to use (most of the time). + */ + public double getAngleRadians() { + return -Math.toRadians(navx.getAngle()); + } + + public double getAccelX() { + return navx.getWorldLinearAccelY(); + } + + public double getAccelY() { + return -navx.getWorldLinearAccelX(); + } + + /** + * This is bad. Do NOT use this. Re-tune your PID if you have to. + * + * @return NavX raw pitch clockwise in degrees + */ + public double getRawPitchDegrees() { + return navx.getPitch(); + } +} diff --git a/src/main/kotlin/org/team1540/rooster/drive/pipeline/PipelineExtensions.kt b/src/main/kotlin/org/team1540/rooster/drive/pipeline/PipelineExtensions.kt index f1cd7bd0..97156881 100644 --- a/src/main/kotlin/org/team1540/rooster/drive/pipeline/PipelineExtensions.kt +++ b/src/main/kotlin/org/team1540/rooster/drive/pipeline/PipelineExtensions.kt @@ -2,14 +2,13 @@ package org.team1540.rooster.drive.pipeline -import org.team1540.rooster.functional.Executable import java.util.function.Consumer import java.util.function.Supplier // somewhat unfortunate names operator fun (() -> T).plus(f: (T) -> Unit): () -> Unit = { f(this()) } -operator fun Supplier.plus(f: Consumer): Executable = Executable { f.accept(this.get()) } +operator fun Supplier.plus(f: Consumer): Runnable = Runnable { f.accept(this.get()) } @JvmName("plusProcessor") operator fun (() -> T).plus(f: (T) -> R): () -> R = { f(this()) } diff --git a/src/testbots/java/org/team1540/rooster/testing/AdjustableTestRobot.java b/src/testbots/java/org/team1540/rooster/testing/AdjustableTestRobot.java deleted file mode 100644 index dce54f64..00000000 --- a/src/testbots/java/org/team1540/rooster/testing/AdjustableTestRobot.java +++ /dev/null @@ -1,52 +0,0 @@ -package org.team1540.rooster.testing; - -import edu.wpi.first.wpilibj.Sendable; -import edu.wpi.first.wpilibj.Solenoid; -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.command.Scheduler; -import org.team1540.rooster.adjustables.AdjustableManager; -import org.team1540.rooster.adjustables.Telemetry; -import org.team1540.rooster.adjustables.Tunable; -import org.team1540.rooster.power.PowerManager; - -@Deprecated -public class AdjustableTestRobot extends TimedRobot { - - @Tunable("A boolean") - public boolean b; - @Tunable("A String") - public String string = "String"; - - @Tunable("A double") - public double d = 2; - @Telemetry("Double times 2 is") - public double dTimes2; - - @Tunable("An int") - public int i = 2; - @Telemetry("Int times 2 is") - public int iTimes2; - - @Telemetry("String plus \"Chickens\" is ") - public String stringPlusChickens = string + " Chickens"; - @Telemetry("NOT(boolean) is") - public boolean notB; - - @Telemetry("Solenoid") - public Sendable sendable = new Solenoid(1); - - @Override - public void robotInit() { - AdjustableManager.getInstance().add(this); - PowerManager.getInstance().interrupt(); - } - - @Override - public void robotPeriodic() { - dTimes2 = d * 2; - iTimes2 = i * 2; - stringPlusChickens = string + "Chickens"; - notB = !b; - Scheduler.getInstance().run(); - } -} diff --git a/src/testbots/java/org/team1540/rooster/testing/AsyncCommandTestRobot.java b/src/testbots/java/org/team1540/rooster/testing/AsyncCommandTestRobot.java deleted file mode 100644 index 5b1c7a83..00000000 --- a/src/testbots/java/org/team1540/rooster/testing/AsyncCommandTestRobot.java +++ /dev/null @@ -1,113 +0,0 @@ -package org.team1540.rooster.testing; - -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.command.Scheduler; -import edu.wpi.first.wpilibj.command.Subsystem; -import org.team1540.rooster.preferencemanager.Preference; -import org.team1540.rooster.util.AsyncCommand; - -public class AsyncCommandTestRobot extends TimedRobot { - - Timer toggleTimer = new Timer(); - Command command1 = new NonAsyncTest(); - Command command2 = new AsyncTest(); - - @Preference(value = "End commands", persistent = false) - public boolean endCommands = false; - - private static Subsystem subsystem = new Subsystem() { - @Override - protected void initDefaultCommand() { - - } - }; - - @Override - public void robotPeriodic() { - Scheduler.getInstance().run(); - if (toggleTimer.get() <= 0) { - toggleTimer.reset(); - toggleTimer.start(); - } - - if (toggleTimer.get() > 1) { - toggleTimer.reset(); - if (command1.isRunning()) { - if (endCommands) { - command1.cancel(); - } - command2.start(); - } else { - if (endCommands) { - command2.cancel(); - } - command1.start(); - } - } - } - - private static class NonAsyncTest extends Command { - - public NonAsyncTest() { - requires(subsystem); - setRunWhenDisabled(true); - } - - @Override - protected void initialize() { - System.out.println(System.currentTimeMillis() + ": Non-async command initialized"); - } - - @Override - protected void execute() { - System.out.println(System.currentTimeMillis() + ": Non-async command executed"); - - } - - @Override - protected boolean isFinished() { - return false; - } - - @Override - protected void end() { - System.out.println(System.currentTimeMillis() + ": Non-async command ended"); - } - - @Override - protected void interrupted() { - System.out.println(System.currentTimeMillis() + ": Non-async command interrupted"); - } - } - - private static class AsyncTest extends AsyncCommand { - - public AsyncTest() { - super(10); - setRunWhenDisabled(true); - requires(subsystem); - } - - @Override - protected void runInitial() { - System.out.println(System.currentTimeMillis() + ": Async command initialized"); - } - - @Override - protected void runEnd() { - System.out.println(System.currentTimeMillis() + ": Async command ended"); - } - - @Override - protected void runInterrupt() { - System.out.println(System.currentTimeMillis() + ": Async command interrupted"); - } - - @Override - protected void runPeriodic() { - System.out.println(System.currentTimeMillis() + ": Async command executed"); - } - } -} diff --git a/src/testbots/java/org/team1540/rooster/testing/BlinkenTestRobot.java b/src/testbots/java/org/team1540/rooster/testing/BlinkenTestRobot.java index 4887e18a..2d0e2f70 100644 --- a/src/testbots/java/org/team1540/rooster/testing/BlinkenTestRobot.java +++ b/src/testbots/java/org/team1540/rooster/testing/BlinkenTestRobot.java @@ -17,10 +17,10 @@ public void robotInit() { boolean defaultSet = false; for (ColorPattern pattern : ColorPattern.values()) { if (!defaultSet) { - patternChooser.addDefault(pattern.name(), pattern); + patternChooser.setDefaultOption(pattern.name(), pattern); defaultSet = true; } else { - patternChooser.addObject(pattern.name(), pattern); + patternChooser.addOption(pattern.name(), pattern); } } diff --git a/src/testbots/java/org/team1540/rooster/testing/PreferencesTestRobot.java b/src/testbots/java/org/team1540/rooster/testing/PreferencesTestRobot.java index f9f0d8e0..83402c79 100644 --- a/src/testbots/java/org/team1540/rooster/testing/PreferencesTestRobot.java +++ b/src/testbots/java/org/team1540/rooster/testing/PreferencesTestRobot.java @@ -3,8 +3,7 @@ import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.command.Scheduler; -import org.team1540.rooster.power.PowerManager; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import org.team1540.rooster.preferencemanager.Preference; import org.team1540.rooster.preferencemanager.PreferenceManager; @@ -38,7 +37,6 @@ public class PreferencesTestRobot extends TimedRobot { @Override public void robotInit() { PreferenceManager.getInstance().add(this); - PowerManager.getInstance().interrupt(); new Notifier(() -> { System.out.println("Current boolean value: " + b); System.out.println("Current String value: " + string); @@ -55,6 +53,6 @@ public void robotInit() { @Override public void robotPeriodic() { - Scheduler.getInstance().run(); + CommandScheduler.getInstance().run(); } } diff --git a/src/testbots/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt b/src/testbots/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt deleted file mode 100644 index b778c2eb..00000000 --- a/src/testbots/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt +++ /dev/null @@ -1,465 +0,0 @@ -package org.team1540.rooster.testing - -import com.ctre.phoenix.motorcontrol.ControlMode -import com.kauailabs.navx.frc.AHRS -import edu.wpi.first.wpilibj.* -import edu.wpi.first.wpilibj.command.Command -import edu.wpi.first.wpilibj.command.Scheduler -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard -import org.team1540.rooster.Utilities -import org.team1540.rooster.drive.pipeline.* -import org.team1540.rooster.functional.Executable -import org.team1540.rooster.functional.Input -import org.team1540.rooster.functional.Processor -import org.team1540.rooster.motionprofiling.ProfileContainer -import org.team1540.rooster.preferencemanager.Preference -import org.team1540.rooster.preferencemanager.PreferenceManager -import org.team1540.rooster.util.SimpleAsyncCommand -import org.team1540.rooster.util.SimpleCommand -import org.team1540.rooster.util.SimpleLoopCommand -import org.team1540.rooster.wrappers.ChickenTalon -import java.io.File -import java.util.OptionalDouble -import java.util.function.DoubleSupplier - -/** - * Base class that all other testing classes inherit from; just has a command that gets started when - * teleop starts. - */ -abstract class DrivePipelineTestRobot : TimedRobot() { - protected abstract val command: Command - - override fun teleopInit() { - command.start() - } - - override fun robotPeriodic() { - Scheduler.getInstance().run() - } -} - -/** - * just to test that everything's sane; joystick tank drive - * */ -class SimpleDrivePipelineTestRobot : DrivePipelineTestRobot() { - override val command = SimpleLoopCommand("Drive", - SimpleJoystickInput(Joystick(0), 1, 5, 3, 2, false, false) + - CTREOutput(PipelineDriveTrain.left1, PipelineDriveTrain.right1) - ) -} - -/** - * Testing class for [AdvancedArcadeJoystickInput]. - */ -class AdvancedJoystickInputPipelineTestRobot : DrivePipelineTestRobot() { - @JvmField - @Preference(persistent = false) - var maxVelocity = 1.0 - @JvmField - @Preference(persistent = false) - var trackWidth = 1.0 - @JvmField - @Preference(persistent = false) - var tpu = 1.0 - @JvmField - @Preference(persistent = false) - var power = 0.0 - - @JvmField - @Preference(persistent = false) - var p = 0.0 - @JvmField - @Preference(persistent = false) - var i = 0.0 - @JvmField - @Preference(persistent = false) - var d = 0.0 - @JvmField - @Preference(persistent = false) - var ramp = 0.0 - @JvmField - @Preference(persistent = false) - var revBack = false - - - private val joystick = XboxController(0) - - override fun robotInit() { - PreferenceManager.getInstance().add(this) - val reset = SimpleCommand("reset", Executable { - _command = SimpleAsyncCommand("Drive", 20, AdvancedArcadeJoystickInput(revBack, - DoubleSupplier { Utilities.scale(-Utilities.processDeadzone(joystick.getY(GenericHID.Hand.kLeft), 0.1), power) }, - DoubleSupplier { Utilities.scale(Utilities.processDeadzone(joystick.getX(GenericHID.Hand.kRight), 0.1), power) }, - DoubleSupplier { - Utilities.scale((Utilities.processDeadzone(joystick.getTriggerAxis(GenericHID.Hand.kRight), 0.1) - - Utilities.processDeadzone(joystick.getTriggerAxis(GenericHID.Hand.kLeft), 0.1)), power) - }) - + FeedForwardToVelocityProcessor(maxVelocity) - + (FeedForwardProcessor(1 / maxVelocity, 0.0, 0.0)) - + UnitScaler(tpu, 0.1) - + (CTREOutput(PipelineDriveTrain.left1, PipelineDriveTrain.right1))) - - PipelineDriveTrain.masters { - configClosedloopRamp(ramp) - config_kP(0, p) - config_kI(0, i) - config_kD(0, d) - config_kF(0, 0.0) - } - - }).apply { - setRunWhenDisabled(true) - start() - } - - SmartDashboard.putData(reset) - } - - private lateinit var _command: Command - override val command get() = _command -} - -class HeadingPIDPipelineTestRobot : DrivePipelineTestRobot() { - - @JvmField - @Preference(persistent = false) - var p = 0.0 - @JvmField - @Preference(persistent = false) - var i = 0.0 - @JvmField - @Preference(persistent = false) - var d = 0.0 - @JvmField - @Preference(persistent = false) - var hdgSet = 0.0 - @JvmField - @Preference(persistent = false) - var invertSides = false - - private var headingPIDProcessor: HeadingPIDProcessor? = null - - override fun robotInit() { - PreferenceManager.getInstance().add(this) - val reset = SimpleCommand("reset", Executable { - headingPIDProcessor = HeadingPIDProcessor(p, i, d, - { Math.toRadians(PipelineNavx.navx.yaw.toDouble()) }, - false, invertSides) - _command = SimpleAsyncCommand("Drive", 20, - Input { - TankDriveData( - DriveData(OptionalDouble.empty()), - DriveData(OptionalDouble.empty()), - OptionalDouble.of(hdgSet), - OptionalDouble.empty()) - } + headingPIDProcessor!! + (CTREOutput(PipelineDriveTrain.left1, PipelineDriveTrain.right1)) - ) - - - }).apply { - setRunWhenDisabled(true) - start() - } - - SmartDashboard.putData(reset) - } - - override fun robotPeriodic() { - super.robotPeriodic() - - headingPIDProcessor?.error?.let { SmartDashboard.putNumber("Error", it) } - headingPIDProcessor?.iAccum?.let { SmartDashboard.putNumber("Iaccum", it) } - SmartDashboard.putNumber("hdg", Math.toRadians(PipelineNavx.navx.yaw.toDouble())) - } - - private lateinit var _command: Command - override val command get() = _command -} - -class TurningRatePIDPipelineTestRobot : DrivePipelineTestRobot() { - - @JvmField - @Preference(persistent = false) - var p = 0.0 - @JvmField - @Preference(persistent = false) - var i = 0.0 - @JvmField - @Preference(persistent = false) - var d = 0.0 - @JvmField - @Preference(persistent = false) - var turnSet = 0.0 - @JvmField - @Preference(persistent = false) - var invertSides = false - - private var turningRatePIDProcessor: TurningRatePIDProcessor? = null - - override fun robotInit() { - PreferenceManager.getInstance().add(this) - val reset = SimpleCommand("reset", Executable { - turningRatePIDProcessor = TurningRatePIDProcessor({ Math.toRadians(PipelineNavx.navx.rate) }, p, i, d, invertSides) - _command = SimpleAsyncCommand("Drive", 20, - Input { - TankDriveData( - DriveData(OptionalDouble.empty()), - DriveData(OptionalDouble.empty()), - OptionalDouble.empty(), - OptionalDouble.of(turnSet)) - } + turningRatePIDProcessor!! + FeedForwardProcessor(1.0, 0.0, 0.0) + (CTREOutput(PipelineDriveTrain.left1, PipelineDriveTrain.right1)) - ) - - - }).apply { - setRunWhenDisabled(true) - start() - } - - SmartDashboard.putData(reset) - } - - override fun robotPeriodic() { - super.robotPeriodic() - - turningRatePIDProcessor?.error?.let { SmartDashboard.putNumber("Error", it) } - turningRatePIDProcessor?.iAccum?.let { SmartDashboard.putNumber("Iaccum", it) } - SmartDashboard.putNumber("hdg", Math.toRadians(PipelineNavx.navx.yaw.toDouble())) - } - - private lateinit var _command: Command - override val command get() = _command -} - -class MotionProfilePipelineTestRobot : DrivePipelineTestRobot() { - @JvmField - @Preference(persistent = false) - var hdgP = 0.0 - @JvmField - @Preference(persistent = false) - var hdgI = 0.0 - @JvmField - @Preference(persistent = false) - var hdgD = 0.0 - @JvmField - @Preference(persistent = false) - var driveP = 0.0 - @JvmField - @Preference(persistent = false) - var driveI = 0.0 - @JvmField - @Preference(persistent = false) - var driveD = 0.0 - @JvmField - @Preference(persistent = false) - var invertSides = false - @JvmField - @Preference(persistent = false) - var kV = 0.0 - @JvmField - @Preference(persistent = false) - var kA = 0.0 - @JvmField - @Preference(persistent = false) - var vIntercept = 0.0 - @JvmField - @Preference(persistent = false) - var profileName = "" - @JvmField - @Preference(persistent = false) - var tpu = 1.0 - @JvmField - @Preference(persistent = false) - var ramp = 0.0 - - private var hdgPIDProcessor: HeadingPIDProcessor? = null - private var profileInput: ProfileInput? = null - private var hdgTgt = 0.0 - private var lastProf: TankDriveData? = null - - private var notifier: Notifier? = null - - override fun robotInit() { - PreferenceManager.getInstance().add(this) - val reset = SimpleCommand("reset", Executable { - val container = ProfileContainer(File("/home/lvuser/roostertest")) - println("Loaded profiles: ${container.profileNames}") - - val profile = container[profileName] - if (profile == null) { - System.err.println("Motion profile not found, aborting reset") - return@Executable - } - - profileInput = ProfileInput(profile.left, profile.right) - - PipelineDriveTrain.masters { setSelectedSensorPosition(0) } - PipelineNavx.navx.zeroYaw() - - hdgPIDProcessor = HeadingPIDProcessor(hdgP, hdgI, hdgD, { Math.toRadians(PipelineNavx.navx.yaw.toDouble()) }, true, invertSides) - _command = SimpleAsyncCommand("Drive", 20, profileInput!! - + FeedForwardProcessor(kV, vIntercept, kA) - + hdgPIDProcessor!! - + Processor { lastProf = it; hdgTgt = it.heading.asDouble;it } - + UnitScaler(tpu, 0.1) - + CTREOutput(PipelineDriveTrain.left1, PipelineDriveTrain.right1) - ) - - PipelineDriveTrain.masters { - config_kP(0, driveP) - config_kI(0, driveI) - config_kD(0, driveD) - config_kF(0, 0.0) - } - PipelineDriveTrain.all { - configClosedloopRamp(ramp) - } - - }).apply { - setRunWhenDisabled(true) - start() - } - - SmartDashboard.putData(reset) - } - - private lateinit var _command: Command - override val command get() = _command - - override fun robotPeriodic() { - super.robotPeriodic() - - hdgPIDProcessor?.let { - SmartDashboard.putNumber("HDG ERR", Math.toDegrees(it.error)) - SmartDashboard.putNumber("HDG IACC", it.iAccum) - } - - lastProf?.let { - SmartDashboard.putNumber("LPOSTGT", it.left.position.orElse(0.0)) - SmartDashboard.putNumber("LVELTGT", it.left.velocity.orElse(0.0)) - SmartDashboard.putNumber("LACCTGT", it.left.acceleration.orElse(0.0)) - SmartDashboard.putNumber("RPOSTGT", it.right.position.orElse(0.0)) - SmartDashboard.putNumber("RVELTGT", it.right.velocity.orElse(0.0)) - SmartDashboard.putNumber("RACCTGT", it.right.acceleration.orElse(0.0)) - SmartDashboard.putNumber("HTGT", Math.toDegrees(hdgTgt)) - } - - SmartDashboard.putNumber("HDG", PipelineNavx.navx.yaw.toDouble()) - SmartDashboard.putNumber("LPOS", PipelineDriveTrain.left1.selectedSensorPosition / tpu) - SmartDashboard.putNumber("RPOS", PipelineDriveTrain.right1.selectedSensorPosition / tpu) - SmartDashboard.putNumber("LVEL", (PipelineDriveTrain.left1.selectedSensorVelocity * 10) / tpu) - SmartDashboard.putNumber("RVEL", (PipelineDriveTrain.right1.selectedSensorVelocity * 10) / tpu) - SmartDashboard.putNumber("LERR", PipelineDriveTrain.left1.closedLoopError / tpu) - SmartDashboard.putNumber("RERR", PipelineDriveTrain.right1.closedLoopError / tpu) - } - - override fun teleopInit() { - super.teleopInit() - - PipelineDriveTrain.masters { setSelectedSensorPosition(0) } - PipelineNavx.navx.zeroYaw() - - PipelineDriveTrain.all { - setBrake(true) - } - } - - override fun disabledInit() { - super.disabledInit() - notifier = Notifier { - Thread.sleep(1000) - PipelineDriveTrain.all { - setBrake(false) - } - } - notifier?.startSingle(1.0) - } -} - -/** - * Common drive train object to be used by all pipeline test robots. - */ -@Suppress("unused") -private object PipelineDriveTrain { - val left1 = ChickenTalon(1).apply { - setBrake(true) - configClosedloopRamp(0.0) - configOpenloopRamp(0.0) - configPeakOutputForward(1.0) - configPeakOutputReverse(-1.0) - enableCurrentLimit(false) - inverted = false - setSensorPhase(true) - } - - private val left2 = ChickenTalon(2).apply { - setBrake(true) - configClosedloopRamp(0.0) - configOpenloopRamp(0.0) - configPeakOutputForward(1.0) - configPeakOutputReverse(-1.0) - enableCurrentLimit(false) - inverted = false - set(ControlMode.Follower, left1.deviceID.toDouble()) - } - private val left3 = ChickenTalon(3).apply { - setBrake(true) - configClosedloopRamp(0.0) - configOpenloopRamp(0.0) - configPeakOutputForward(1.0) - configPeakOutputReverse(-1.0) - enableCurrentLimit(false) - inverted = false - set(ControlMode.Follower, left1.deviceID.toDouble()) - } - - val right1 = ChickenTalon(4).apply { - setBrake(true) - configClosedloopRamp(0.0) - configOpenloopRamp(0.0) - configPeakOutputForward(1.0) - configPeakOutputReverse(-1.0) - enableCurrentLimit(false) - inverted = true - setSensorPhase(true) - } - private val right2 = ChickenTalon(5).apply { - setBrake(true) - configClosedloopRamp(0.0) - configOpenloopRamp(0.0) - configPeakOutputForward(1.0) - configPeakOutputReverse(-1.0) - enableCurrentLimit(false) - inverted = true - set(ControlMode.Follower, right1.deviceID.toDouble()) - } - private val right3 = ChickenTalon(6).apply { - setBrake(true) - configClosedloopRamp(0.0) - configOpenloopRamp(0.0) - configPeakOutputForward(1.0) - configPeakOutputReverse(-1.0) - enableCurrentLimit(false) - inverted = true - set(ControlMode.Follower, right1.deviceID.toDouble()) - } - - fun all(block: ChickenTalon.() -> Unit) { - for (talon in listOf(left1, left2, left3, right1, right2, right3)) { - talon.block() - } - } - - fun masters(block: ChickenTalon.() -> Unit) { - for (talon in listOf(left1, right1)) { - talon.block() - } - } -} - -/** - * Just an object to hold a NavX for pipeline testers. - */ -private object PipelineNavx { - val navx = AHRS(SPI.Port.kMXP) -} diff --git a/src/testbots/kotlin/org/team1540/rooster/testing/ProfileContainerTestingRobot.kt b/src/testbots/kotlin/org/team1540/rooster/testing/ProfileContainerTestingRobot.kt deleted file mode 100644 index a422f917..00000000 --- a/src/testbots/kotlin/org/team1540/rooster/testing/ProfileContainerTestingRobot.kt +++ /dev/null @@ -1,18 +0,0 @@ -package org.team1540.rooster.testing - -import edu.wpi.first.wpilibj.TimedRobot -import org.team1540.rooster.motionprofiling.ProfileContainer -import java.io.File -import kotlin.system.measureTimeMillis - -class ProfileContainerTestingRobot : TimedRobot() { - private lateinit var container: ProfileContainer - - override fun robotInit() { - val time = measureTimeMillis { - container = ProfileContainer(File("/home/lvuser/roostertest")) - } - println("Initialized profile container in $time ms") - println("Current profiles loaded: ${container.profileNames}") - } -} diff --git a/vendordeps/PathfinderOLD.json b/vendordeps/PathfinderOLD.json deleted file mode 100644 index f7157aef..00000000 --- a/vendordeps/PathfinderOLD.json +++ /dev/null @@ -1,87 +0,0 @@ -{ - "fileName": "PathfinderOLD.json", - "name": "PathfinderOld", - "version": "2019.1.10", - "uuid": "7194a2d4-2860-4bcc-86c0-97879737d875", - "uuid": "7194a2d4-2860-4bcc-86c0-97879737d875", - "mavenUrls": [ - "https://dev.imjac.in/maven" - ], - "jsonUrl": "https://dev.imjac.in/maven/jaci/pathfinder/PathfinderOLD-latest.json", - "cppDependencies": [ - { - "groupId": "jaci.pathfinder", - "artifactId": "Pathfinder-Core", - "version": "2019.1.10", - "libName": "pathfinder", - "configuration": "native_pathfinder_old", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxx86-64", - "windowsx86-64", - "osxx86-64", - "linuxathena", - "linuxraspbian" - ] - }, - { - "groupId": "jaci.pathfinder", - "artifactId": "Pathfinder-FRCSupport", - "version": "2019.1.10", - "libName": "pathfinder_frc", - "configuration": "native_pathfinder_old", - "headerClassifier": "headers", - "binaryPlatforms": [ - ] - } - ], - "javaDependencies": [ - { - "groupId": "jaci.pathfinder", - "artifactId": "Pathfinder-Java", - "version": "2019.1.10" - }, - { - "groupId": "jaci.jniloader", - "artifactId": "JNILoader", - "version": "1.0.1" - }, - { - "groupId": "jaci.pathfinder", - "artifactId": "Pathfinder-FRCSupport", - "version": "2019.1.10" - } - ], - "jniDependencies": [ - { - "groupId": "jaci.pathfinder", - "artifactId": "Pathfinder-JNI", - "version": "2019.1.10", - "isJar": true, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxx86-64", - "windowsx86-64", - "osxx86-64", - "linuxathena", - "linuxraspbian" - ] - }, - { - "groupId": "jaci.pathfinder", - "artifactId": "Pathfinder-CoreJNI", - "version": "2019.1.10", - "isJar": true, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxx86-64", - "windowsx86-64", - "osxx86-64", - "linuxathena", - "linuxraspbian" - ] - } - ] -} diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index df8d7600..d32d534d 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,87 +1,180 @@ { - "fileName": "Phoenix.json", - "name": "CTRE-Phoenix", - "version": "5.12.0", - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", - "mavenUrls": [ - "http://devsite.ctr-electronics.com/maven/release/" - ], - "jsonUrl": "http://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json", - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.12.0" + "fileName": "Phoenix.json", + "name": "CTRE-Phoenix", + "version": "5.17.2", + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "http://devsite.ctr-electronics.com/maven/release/" + ], + "jsonUrl": "http://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json", + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.17.2" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.12.0" + "version": "5.17.2" } ], "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.12.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - } + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.17.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "diagnostics", + "version": "5.17.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "canutils", + "version": "5.17.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "platform-stub", + "version": "5.17.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "core", + "version": "5.17.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + } ], "cppDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-cpp", - "version": "5.12.0", - "libName": "CTRE_Phoenix_WPI", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.12.0", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.12.0", - "libName": "CTRE_PhoenixCCI", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "core", - "version": "5.12.0", - "libName": "CTRE_PhoenixCore", - "headerClassifier": "headers" - } + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.17.2", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.17.2", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.17.2", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "diagnostics", + "version": "5.17.2", + "libName": "CTRE_PhoenixDiagnostics", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "canutils", + "version": "5.17.2", + "libName": "CTRE_PhoenixCanutils", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "platform-stub", + "version": "5.17.2", + "libName": "CTRE_PhoenixPlatform", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "core", + "version": "5.17.2", + "libName": "CTRE_PhoenixCore", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + } ] } diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json new file mode 100644 index 00000000..d72bdce2 --- /dev/null +++ b/vendordeps/WPILibNewCommands.json @@ -0,0 +1,37 @@ +{ + "fileName": "WPILibNewCommands.json", + "name": "WPILib-New-Commands", + "version": "2020.0.0", + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "linuxaarch64bionic", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxx86-64" + ] + } + ] +} diff --git a/vendordeps/navx_frc.json b/vendordeps/navx_frc.json index 255358b6..ed47b105 100644 --- a/vendordeps/navx_frc.json +++ b/vendordeps/navx_frc.json @@ -1,33 +1,34 @@ { - "fileName": "navx_frc.json", - "name": "KauaiLabs_navX_FRC", - "version": "3.1.344", - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "mavenUrls": [ - "https://repo1.maven.org/maven2/" - ], - "jsonUrl": "https://www.kauailabs.com/dist/frc/2019/navx_frc.json", - "javaDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-java", - "version": "3.1.344" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-cpp", - "version": "3.1.344", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": false, - "libName": "navx_frc", - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena" - ] - } + "fileName": "navx_frc.json", + "name": "KauaiLabs_navX_FRC", + "version": "3.1.400", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "mavenUrls": [ + "https://repo1.maven.org/maven2/" + ], + "jsonUrl": "https://www.kauailabs.com/dist/frc/2020/navx_frc.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-java", + "version": "3.1.400" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-cpp", + "version": "3.1.400", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian" + ] + } ] }