feat(math/curves): implement abstract Curve class and multiple different curve types.

build.gradle

@@ -44,6 +44,7 @@ dependencies {
     compileOnly "edu.wpi.first.wpilibj:wpilibj-java:2024.3.2"
     compileOnly "edu.wpi.first.wpiutil:wpiutil-java:2024.3.2"
     compileOnly "edu.wpi.first.wpilibj:commands:2024.3.2"
+    compileOnly "edu.wpi.first.wpimath:wpimath-java:2024.3.2"
     compileOnly "edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2024.3.2"
 
     // PathPlannerLib

src/main/java/net/frc5183/librobot/math/curve/Curve.java

@@ -0,0 +1,13 @@
+package net.frc5183.librobot.math.curve;
+
+/**
+ * An abstract class which represents a mathematical function/curve.
+ */
+public abstract class Curve {
+    /**
+     * Returns the value of the curve at the given x value.
+     * @param x The x value to evaluate the curve at.
+     * @return The value of the curve at the given x value.
+     */
+    public abstract double curve(double x);
+}

src/main/java/net/frc5183/librobot/math/curve/DoubleTimedCurve.java

@@ -0,0 +1,76 @@
+package net.frc5183.librobot.math.curve;
+
+import org.jetbrains.annotations.NotNull;
+
+/**
+ * A {@link TimedCurve} that toggles between two curves.
+ */
+public class DoubleTimedCurve extends TimedCurve {
+    /**
+     * The curve to use when the curve is enabled.
+     */
+    @NotNull
+    private Curve enabledCurve;
+
+    /**
+     * The curve to use when the curve is disabled.
+     */
+    @NotNull
+    private Curve disabledCurve;
+
+    /**
+     * Creates a new {@link DoubleTimedCurve} with the given curves and delays.
+     * @param enabledCurve The curve to use when this curve is enabled.
+     * @param disabledCurve The curve to use when this curve is disabled.
+     * @param delayEnabled The delay in seconds before the curve is enabled.
+     * @param delayDisabled The delay in seconds before the curve is disabled.
+     * @see TimedCurve#TimedCurve(double, double)
+     */
+    public DoubleTimedCurve(@NotNull Curve enabledCurve, @NotNull Curve disabledCurve, double delayEnabled, double delayDisabled) {
+        super(delayEnabled, delayDisabled);
+        this.enabledCurve = enabledCurve;
+        this.disabledCurve = disabledCurve;
+    }
+
+    @Override
+    protected double enabled(double x) {
+        return enabledCurve.curve(x);
+    }
+
+    @Override
+    protected double disabled(double x) {
+        return disabledCurve.curve(x);
+    }
+
+    /**
+     * Returns the curve to use when this curve is enabled.
+     * @return The curve to use when this curve is enabled.
+     */
+    public @NotNull Curve getEnabledCurve() {
+        return enabledCurve;
+    }
+
+    /**
+     * Sets the curve to use when this curve is enabled.
+     * @param enabledCurve The new curve to use when the curve is enabled.
+     */
+    public void setEnabledCurve(@NotNull Curve enabledCurve) {
+        this.enabledCurve = enabledCurve;
+    }
+
+    /**
+     * Returns the curve to use when this curve is disabled.
+     * @return The curve to use when this curve is disabled.
+     */
+    public @NotNull Curve getDisabledCurve() {
+        return disabledCurve;
+    }
+
+    /**
+     * Sets the curve to use when this curve is disabled.
+     * @param disabledCurve The new curve to use when the curve is disabled.
+     */
+    public void setDisabledCurve(@NotNull Curve disabledCurve) {
+        this.disabledCurve = disabledCurve;
+    }
+}

src/main/java/net/frc5183/librobot/math/curve/ExponentialCurve.java

@@ -0,0 +1,54 @@
+package net.frc5183.librobot.math.curve;
+
+/**
+ * An {@link Curve} which represents an exponential function in the form y = (x/|x|) * ((1 + k)^|x| - 1) / k.
+ * Where k is the exaggeration of the curve.
+ */
+public class ExponentialCurve extends Curve {
+    /**
+     * The exaggeration of the curve.
+     */
+    private double exaggeration;
+
+    /**
+     * Creates a new {@link ExponentialCurve} with the given exaggeration.
+     * @param exaggeration The exaggeration of the curve.
+     * @throws IllegalArgumentException If the exaggeration is less than or equal to 0.
+     */
+    public ExponentialCurve(double exaggeration) {
+        if (exaggeration <= 0) {
+            throw new IllegalArgumentException("Exaggeration must be greater than 0.");
+        }
+
+        this.exaggeration = exaggeration;
+    }
+
+    @Override
+    public double curve(double x) {
+        if (x == 0) return 0d;
+
+        // y = (x/|x|) * ((1 + exaggeration)^|x| - 1) / exaggeration
+        return (x / Math.abs(x)) * ((Math.pow(1 + exaggeration, Math.abs(x))) - 1) / exaggeration; // NOPMD - extra parentheses make the equation easier to read
+    }
+
+    /**
+     * Returns the exaggeration of the curve.
+     * @return The exaggeration of the curve.
+     */
+    public double getExaggeration() {
+        return exaggeration;
+    }
+
+    /**
+     * Sets the exaggeration of the curve.
+     * @param exaggeration The new exaggeration of the curve.
+     * @throws IllegalArgumentException If the exaggeration is less than or equal to 0.
+     */
+    public void setExaggeration(double exaggeration) {
+        if (exaggeration <= 0) {
+            throw new IllegalArgumentException("Exaggeration must be greater than 0.");
+        }
+
+        this.exaggeration = exaggeration;
+    }
+}

src/main/java/net/frc5183/librobot/math/curve/LimitedCurve.java

@@ -0,0 +1,97 @@
+package net.frc5183.librobot.math.curve;
+
+import org.jetbrains.annotations.NotNull;
+
+/**
+ * A {@link Curve} which limits the output of another curve to a certain range.
+ */
+public class LimitedCurve extends Curve {
+    /**
+     * The curve to limit.
+     */
+    @NotNull
+    private Curve curve;
+
+    /**
+     * The minimum value of the curve.
+     */
+    private double min;
+
+    /**
+     * The maximum value of the curve.
+     */
+    private double max;
+
+    /**
+     * Creates a new {@link LimitedCurve} with the given curve, minimum, and maximum values.
+     * @param curve The curve to limit.
+     * @param min The minimum value of the curve.
+     * @param max The maximum value of the curve.
+     * @throws IllegalArgumentException If the minimum value is greater than the maximum value.
+     */
+    public LimitedCurve(@NotNull Curve curve, double min, double max) {
+        if (min > max) throw new IllegalArgumentException("Minimum value cannot be greater than maximum value.");
+
+        this.curve = curve;
+        this.min = min;
+        this.max = max;
+    }
+
+    @Override
+    public double curve(double x) {
+        double y = curve.curve(x);
+        return Math.min(Math.max(y, min), max);
+    }
+
+    /**
+     * Returns the curve to limit.
+     * @return The curve to limit.
+     */
+    public @NotNull Curve getCurve() {
+        return curve;
+    }
+
+    /**
+     * Sets the curve to limit.
+     * @param curve The new curve to limit.
+     */
+    public void setCurve(@NotNull Curve curve) {
+        this.curve = curve;
+    }
+
+    /**
+     * Returns the minimum value of the curve.
+     * @return The minimum value of the curve.
+     */
+    public double getMin() {
+        return min;
+    }
+
+    /**
+     * Sets the minimum value of the curve.
+     * @param min The new minimum value of the curve.
+     * @throws IllegalArgumentException If the minimum value is greater than the maximum value.
+     */
+    public void setMin(double min) {
+        if (min > max) throw new IllegalArgumentException("Minimum value cannot be greater than maximum value.");
+        this.min = min;
+    }
+
+    /**
+     * Returns the maximum value of the curve.
+     * @return The maximum value of the curve.
+     */
+    public double getMax() {
+        return max;
+    }
+
+    /**
+     * Sets the maximum value of the curve.
+     * @param max The new maximum value of the curve.
+     * @throws IllegalArgumentException If the minimum value is greater than the maximum value.
+     */
+    public void setMax(double max) {
+        if (min > max) throw new IllegalArgumentException("Minimum value cannot be greater than maximum value.");
+        this.max = max;
+    }
+}

src/main/java/net/frc5183/librobot/math/curve/LinearCurve.java

@@ -0,0 +1,64 @@
+package net.frc5183.librobot.math.curve;
+
+/**
+ * A {@link Curve} which represents a linear equation in the form y = mx + b.
+ */
+public class LinearCurve extends Curve {
+    /**
+     * The slope of the curve.
+     */
+    private double slope;
+
+    /**
+     * The y-intercept of the curve.
+     */
+    private double yIntercept;
+
+    /**
+     * Creates a new {@link LinearCurve} with the given slope and y-intercept.
+     * @param slope The slope of the curve.
+     * @param yIntercept The y-intercept of the curve.
+     */
+    public LinearCurve(double slope, double yIntercept) {
+        this.slope = slope;
+        this.yIntercept = yIntercept;
+    }
+
+    @Override
+    public double curve(double x) {
+        // y = mx + b
+        return slope * x + yIntercept;
+    }
+
+    /**
+     * Returns the slope of the curve.
+     * @return The slope of the curve.
+     */
+    public double getSlope() {
+        return slope;
+    }
+
+    /**
+     * Sets the slope of the curve.
+     * @param slope The new slope of the curve.
+     */
+    public void setSlope(double slope) {
+        this.slope = slope;
+    }
+
+    /**
+     * Returns the y-intercept of the curve.
+     * @return The y-intercept of the curve.
+     */
+    public double getYIntercept() {
+        return yIntercept;
+    }
+
+    /**
+     * Sets the y-intercept of the curve.
+     * @param yIntercept The new y-intercept of the curve.
+     */
+    public void setYIntercept(double yIntercept) {
+        this.yIntercept = yIntercept;
+    }
+}

src/main/java/net/frc5183/librobot/math/curve/NormalTimedCurve.java

@@ -0,0 +1,52 @@
+package net.frc5183.librobot.math.curve;
+
+import org.jetbrains.annotations.NotNull;
+
+/**
+ * A @{link TimedCurve} that toggles between x (when disabled) and curve(x) (when enabled).
+ */
+public class NormalTimedCurve extends TimedCurve {
+    /**
+     * The curve to use when this curve is enabled.
+     */
+    @NotNull
+    private Curve curve;
+
+    /**
+     * Creates a new {@link NormalTimedCurve} with the given curve and delays.
+     * @param curve The curve to use when this curve is enabled.
+     * @param delayEnabled The delay in seconds before the curve is enabled.
+     * @param delayDisabled The delay in seconds before the curve is disabled.
+     */
+    public NormalTimedCurve(@NotNull Curve curve, double delayEnabled, double delayDisabled) {
+        super(delayEnabled, delayDisabled);
+        this.curve = curve;
+    }
+
+    @Override
+    protected double enabled(double x) {
+        return curve(x);
+    }
+
+    @Override
+    protected double disabled(double x) {
+        return x;
+    }
+
+    /**
+     * Returns the curve to use when this curve is enabled.
+     * @return The curve to use when this curve is enabled.
+     */
+    @NotNull
+    public Curve getCurve() {
+        return curve;
+    }
+
+    /**
+     * Sets the curve to use when this curve is enabled.
+     * @param curve The new curve to use when the curve is enabled.
+     */
+    public void setCurve(@NotNull Curve curve) {
+        this.curve = curve;
+    }
+}