Add passing to ShooterFunctions

src/main/java/frc/robot/auto/ShooterFunctions.java

@@ -16,21 +16,63 @@
 import org.apache.commons.math3.analysis.interpolation.SplineInterpolator;
 
 public class ShooterFunctions {
+  /**
+   * Path to the CSV file containing 2D hood angle calibration data for hub shots. Formatted as
+   * Distance, Velocity, Angle
+   */
+  private static final String anglePathHub = "hoodangledatahub.csv";
+
+  /**
+   * Path to the CSV file containing 2-column flywheel velocity calibration data for hub shots.
+   * Formatted as Distance, Velocity
+   */
+  private static final String velocityPathHub = "flywheelvelocitydatahub.csv";
+
+  /**
+   * Path to the CSV file containing hood angle calibration data for passing. Formatted as Distance,
+   * Angle
+   */
+  private static final String anglePathPass = "passinghoodangledata.csv";
+
+  /**
+   * Path to the CSV file containing flywheel velocity calibration data for passing. Formatted as
+   * Distance, Velocity
+   */
+  private static final String velocityPathPass = "passingflywheelvelocitydata.csv";
+
+  /**
+   * Path to the CSV file containing minimum and maximum velocities to shoot at. Formatted as
+   * Distance, Velocity, Angle
+   */
+  private static final String velocityRangeHub = "velocityrangehub.csv";
+
+  /** Minimum valid distance to the target for hub shots, in inches. */
+  private static final double minDistanceHub = 57.5; // Calibrated for our shooter A testing
 
-  private static final String anglePath = "hoodangledata.csv";
-  private static final String velocityPath = "flywheelvelocitydata.csv";
-  private static final double minDistance = 57.5;
-  private static final double maxDistance = 263.0;
+  /** Maximum valid distance to the target for hub shots, in inches. */
+  private static final double maxDistanceHub = 263.0; // Calibrated for our shooter A testing
 
-  private MultivariateFunction hoodAngleFunction;
-  private UnivariateFunction flywheelVelocityFunction;
+  /** Minimum valid distance to the target for passing, in inches. */
+  private static final double minDistancePass = 57.5; // Needs to be changed
+
+  /** Maximum valid distance to the target for passing, in inches. */
+  private static final double maxDistancePass = 263.0; // Needs to be changed
+
+  private MultivariateFunction hoodAngleFunctionHub;
+  private UnivariateFunction flywheelVelocityFunctionHub;
+  private UnivariateFunction hoodAngleFunctionPass;
+  private UnivariateFunction flywheelVelocityFunctionPass;
+  private UnivariateFunction minVelocityHub;
+  private UnivariateFunction maxVelocityHub;
 
   public ShooterFunctions() {
     try {
-      this.hoodAngleFunction = loadShooterData();
-      this.flywheelVelocityFunction = loadFlywheelData();
-      loadShooterData();
-      loadFlywheelData();
+      this.hoodAngleFunctionHub = loadHoodAngleDataHub();
+      this.flywheelVelocityFunctionHub = loadFlywheelDataHub();
+      this.hoodAngleFunctionPass = loadHoodAngleDataPass();
+      this.flywheelVelocityFunctionPass = loadFlywheelDataPass();
+      this.minVelocityHub = loadMinVelocityHub();
+      this.maxVelocityHub = loadMaxVelocityHub();
     } catch (IOException e) {
       e.printStackTrace();
     }
@@ -46,9 +88,9 @@ public ShooterFunctions() {
    *     output using the data loaded from the CSV file
    * @throws IOException if an I/O error occurs while reading the CSV file at {@code path}
    */
-  private MultivariateFunction loadShooterData() throws IOException {
+  private MultivariateFunction loadHoodAngleDataHub() throws IOException {
     MicrosphereInterpolator interpolator = new MicrosphereInterpolator();
-    double[][] data = CSVLoader.loadCSV(anglePath);
+    double[][] data = CSVLoader.loadCSV(anglePathHub);
     double[][] x = new double[data.length][2];
     for (int i = 0; i < data.length; i++) {
       x[i][0] = data[i][0];
@@ -69,9 +111,54 @@ private MultivariateFunction loadShooterData() throws IOException {
    *     (f(x) ≈ y)
    * @throws IOException if an I/O error occurs while reading the CSV file at {@code path2}
    */
-  private UnivariateFunction loadFlywheelData() throws IOException {
+  private UnivariateFunction loadFlywheelDataHub() throws IOException {
+    SplineInterpolator interpolator = new SplineInterpolator();
+    double[][] data = CSVLoader.loadCSV(velocityPathHub);
+    double[] x = new double[data.length];
+    for (int i = 0; i < data.length; i++) {
+      x[i] = data[i][0];
+    }
+    double[] y = new double[data.length];
+    for (int i = 0; i < data.length; i++) {
+      y[i] = data[i][1];
+    }
+    return interpolator.interpolate(x, y);
+  }
+
+  /**
+   * Loads shooter calibration data for passing from a CSV file and returns an interpolating
+   * function.
+   *
+   * @return a {@code UnivariateFunction} that interpolates the distance to a hood angle setpoint
+   *     using the data loaded from the CSV file
+   * @throws IOException if an I/O error occurs while reading the CSV file at {@code anglePathPass}
+   */
+  private UnivariateFunction loadHoodAngleDataPass() throws IOException {
+    SplineInterpolator interpolator = new SplineInterpolator();
+    double[][] data = CSVLoader.loadCSV(anglePathPass);
+    double[] x = new double[data.length];
+    for (int i = 0; i < data.length; i++) {
+      x[i] = data[i][0];
+    }
+    double[] y = new double[data.length];
+    for (int i = 0; i < data.length; i++) {
+      y[i] = data[i][2];
+    }
+    return interpolator.interpolate(x, y);
+  }
+
+  /**
+   * Loads flywheel velocity calibration data for passing from a CSV file and returns an
+   * interpolating function.
+   *
+   * @return a {@code UnivariateFunction} that interpolates the distance to a flywheel velocity
+   *     setpoint using the data loaded from the CSV file
+   * @throws IOException if an I/O error occurs while reading the CSV file at {@code
+   *     velocityPathPass}
+   */
+  private UnivariateFunction loadFlywheelDataPass() throws IOException {
     SplineInterpolator interpolator = new SplineInterpolator();
-    double[][] data = CSVLoader.loadCSV(velocityPath);
+    double[][] data = CSVLoader.loadCSV(velocityPathPass);
     double[] x = new double[data.length];
     for (int i = 0; i < data.length; i++) {
       x[i] = data[i][0];
@@ -84,13 +171,92 @@ private UnivariateFunction loadFlywheelData() throws IOException {
   }
 
   /**
-   * Returns the MultivariateFunction used by the shooter subsystem to compute shooter setpoints.
+   * Loads minimum flywheel velocity data for hub shots from a CSV file and returns an interpolating
+   * function.
    *
-   * @return the configured MultivariateFunction used to compute shooter setpoints, or {@code null}
-   *     if no function has been configured
+   * @return a {@code UnivariateFunction} that interpolates the distance to a velocity setpoint
+   *     using the data loaded from the CSV file
+   * @throws IOException if an I/O error occurs while reading the CSV file at {@code
+   *     velocityRangeHub}
    */
-  public MultivariateFunction getHoodAngleFunction() {
-    return hoodAngleFunction;
+  private UnivariateFunction loadMinVelocityHub() throws IOException {
+    SplineInterpolator interpolator = new SplineInterpolator();
+    double[][] data = CSVLoader.loadCSV(velocityRangeHub);
+    double[] x = new double[data.length];
+    for (int i = 0; i < data.length; i++) {
+      x[i] = data[i][0];
+    }
+    double[] y = new double[data.length];
+    for (int i = 0; i < data.length; i++) {
+      y[i] = data[i][1];
+    }
+    return interpolator.interpolate(x, y);
+  }
+
+  /**
+   * Loads maximum flywheel velocity data for hub shots from a CSV file and returns an interpolating
+   * function.
+   *
+   * @return a {@code UnivariateFunction} that interpolates the distance to a velocity setpoint
+   *     using the data loaded from the CSV file
+   * @throws IOException if an I/O error occurs while reading the CSV file at {@code
+   *     velocityRangeHub}
+   */
+  private UnivariateFunction loadMaxVelocityHub() throws IOException {
+    SplineInterpolator interpolator = new SplineInterpolator();
+    double[][] data = CSVLoader.loadCSV(velocityRangeHub);
+    double[] x = new double[data.length];
+    for (int i = 0; i < data.length; i++) {
+      x[i] = data[i][0];
+    }
+    double[] y = new double[data.length];
+    for (int i = 0; i < data.length; i++) {
+      y[i] = data[i][2];
+    }
+    return interpolator.interpolate(x, y);
+  }
+
+  /**
+   * Returns the MultivariateFunction used by the shooter subsystem to compute hood angle setpoints.
+   *
+   * @return the configured MultivariateFunction used to compute hood angle setpoints, or {@code
+   *     null} if no function has been configured
+   */
+  public MultivariateFunction getHoodAngleFunctionHub() {
+    return hoodAngleFunctionHub;
+  }
+
+  /**
+   * Returns the UnivariateFunction used by the shooter subsystem to compute flywheel velocity
+   * setpoints.
+   *
+   * @return the configured UnivariateFunction used to compute flywheel velocity setpoints, or
+   *     {@code null} if no function has been configured
+   */
+  public UnivariateFunction getFlywheelVelocityFunctionHub() {
+    return flywheelVelocityFunctionHub;
+  }
+
+  /**
+   * Returns the UnivariateFunction used by the shooter subsystem to compute hood angle setpoints
+   * for passing.
+   *
+   * @return the configured UnivariateFunction used to compute hood angle setpoints for passing, or
+   *     {@code null} if no function has been configured
+   */
+  public UnivariateFunction getHoodAngleFunctionPass() {
+    return hoodAngleFunctionPass;
+  }
+
+  /**
+   * Returns the UnivariateFunction used by the shooter subsystem to compute flywheel velocity
+   * setpoints for passing.
+   *
+   * @return the configured UnivariateFunction used to compute flywheel velocity setpoints for
+   *     passing, or {@code null} if no function has been configured
+   */
+  public UnivariateFunction getFlywheelVelocityFunctionPass() {
+    return flywheelVelocityFunctionPass;
   }
 
   /**
@@ -104,19 +270,28 @@ public MultivariateFunction getHoodAngleFunction() {
    *     for the internal calculation)
    * @return an Angle representing the hood setpoint in degrees needed for the shot
    */
-  public Angle getHoodAngle(Distance distance) {
-    if (isWithinValidRange(distance)) {
+  public Angle getHoodAngleHub(Distance distance, AngularVelocity velocity) {
+    if (isDistanceWithinValidHubRange(distance)
+        && isVelocityWithinValidHubRange(distance, velocity)) {
       return Degrees.of(
-          hoodAngleFunction.value(
+          hoodAngleFunctionHub.value(
+              new double[] {distance.in(Inches), velocity.in(RotationsPerSecond)}));
+    } else if (isVelocityWithinValidHubRange(distance, velocity)) {
+      return Degrees.of(
+          hoodAngleFunctionHub.value(
               new double[] {
-                distance.in(Inches), getFlywheelVelocity(distance).in(RotationsPerSecond)
+                MathUtil.clamp(distance.in(Inches), minDistanceHub, maxDistanceHub),
+                velocity.in(RotationsPerSecond)
               }));
     } else
       return Degrees.of(
-          hoodAngleFunction.value(
+          hoodAngleFunctionHub.value(
               new double[] {
-                MathUtil.clamp(distance.in(Inches), minDistance, maxDistance),
-                getFlywheelVelocity(distance).in(RotationsPerSecond)
+                MathUtil.clamp(distance.in(Inches), minDistanceHub, maxDistanceHub),
+                MathUtil.clamp(
+                    velocity.in(RotationsPerSecond),
+                    minVelocityHub.value(distance.in(Inches)),
+                    maxVelocityHub.value(distance.in(Inches)))
               }));
   }
 
@@ -129,13 +304,49 @@ public Angle getHoodAngle(Distance distance) {
    * @param angle the shooter/target angle (currently unused by this method)
    * @return the required flywheel angular velocity as an AngularVelocity (rotations per second)
    */
-  public AngularVelocity getFlywheelVelocity(Distance distance) {
-    if (isWithinValidRange(distance)) {
-      return RotationsPerSecond.of(flywheelVelocityFunction.value(distance.in(Inches)));
+  public AngularVelocity getFlywheelVelocityHub(Distance distance) {
+    if (isDistanceWithinValidHubRange(distance)) {
+      return RotationsPerSecond.of(flywheelVelocityFunctionHub.value(distance.in(Inches)));
     } else
       return RotationsPerSecond.of(
-          flywheelVelocityFunction.value(
-              MathUtil.clamp(distance.in(Inches), minDistance, maxDistance)));
+          flywheelVelocityFunctionHub.value(
+              MathUtil.clamp(distance.in(Inches), minDistanceHub, maxDistanceHub)));
+  }
+
+  /**
+   * Calculates the hood angle required for passing given the current distance to the target. Clamps
+   * the output to the maximum and minimum hood angles if the input distance is outside the valid
+   * range.
+   *
+   * @param distance the distance to the target (will be converted to inches for the internal
+   *     calculation)
+   * @return an Angle representing the hood setpoint in degrees needed for passing
+   */
+  public Angle getHoodAnglePass(Distance distance) {
+    if (isDistanceWithinValidPassRange(distance)) {
+      return Degrees.of(hoodAngleFunctionPass.value(distance.in(Inches)));
+    } else
+      return Degrees.of(
+          hoodAngleFunctionPass.value(
+              MathUtil.clamp(distance.in(Inches), minDistancePass, maxDistancePass)));
+  }
+
+  /**
+   * Calculates the required flywheel angular velocity for passing given the current distance to the
+   * target. Clamps the output to the maximum and minimum flywheel velocities if the input distance
+   * is outside the valid range.
+   *
+   * @param distance the distance to the target; will be converted to inches before evaluation
+   * @return the required flywheel angular velocity for passing as an AngularVelocity (rotations per
+   *     second)
+   */
+  public AngularVelocity getFlywheelVelocityPass(Distance distance) {
+    if (isDistanceWithinValidPassRange(distance)) {
+      return RotationsPerSecond.of(flywheelVelocityFunctionPass.value(distance.in(Inches)));
+    } else
+      return RotationsPerSecond.of(
+          flywheelVelocityFunctionPass.value(
+              MathUtil.clamp(distance.in(Inches), minDistancePass, maxDistancePass)));
   }
 
   /**
@@ -145,8 +356,26 @@ public AngularVelocity getFlywheelVelocity(Distance distance) {
    * @return true if the distance (in inches) is greater than or equal to minDistance and less than
    *     or equal to maxDistance
    */
-  public boolean isWithinValidRange(Distance distance) {
+  public boolean isDistanceWithinValidHubRange(Distance distance) {
+    double distanceInInches = distance.in(Inches);
+    return distanceInInches >= minDistanceHub && distanceInInches <= maxDistanceHub;
+  }
+
+  public boolean isVelocityWithinValidHubRange(Distance distance, AngularVelocity velocity) {
+    double velocityInRPS = velocity.in(RotationsPerSecond);
+    return velocityInRPS >= minVelocityHub.value(distance.in(Inches))
+        && velocityInRPS <= maxVelocityHub.value(distance.in(Inches));
+  }
+
+  /**
+   * Determines whether the given distance falls within the configured valid range for passing.
+   *
+   * @param distance the Distance to check; will be converted to inches for comparison
+   * @return true if the distance (in inches) is greater than or equal to minDistancePass and less
+   *     than or equal to maxDistancePass
+   */
+  public boolean isDistanceWithinValidPassRange(Distance distance) {
     double distanceInInches = distance.in(Inches);
-    return distanceInInches >= minDistance && distanceInInches <= maxDistance;
+    return distanceInInches >= minDistancePass && distanceInInches <= maxDistancePass;
   }
 }