Shooter Interpolation Table

Disclaimer: WIP and untested; populate with real data. Custom code built on WPILib APIs.

Code (Java)

public class ShooterTable {
  private final TreeMap<Double, ShooterSetpoint> table = new TreeMap<>();

  public ShooterTable() {
    // distance meters -> rpm/angle. TODO: populate with tested values
    table.put(2.0, new ShooterSetpoint(3500, 15));
    table.put(3.0, new ShooterSetpoint(3800, 18));
    table.put(4.0, new ShooterSetpoint(4200, 22));
  }

  public ShooterSetpoint get(double distanceMeters) {
    var floor = table.floorEntry(distanceMeters);
    var ceil = table.ceilingEntry(distanceMeters);
    if (floor == null) return ceil.getValue();
    if (ceil == null) return floor.getValue();
    if (floor.getKey().equals(ceil.getKey())) return floor.getValue();
    double t = (distanceMeters - floor.getKey()) / (ceil.getKey() - floor.getKey());
    double rpm = MathUtil.interpolate(floor.getValue().rpm(), ceil.getValue().rpm(), t);
    double angle = MathUtil.interpolate(floor.getValue().angle(), ceil.getValue().angle(), t);
    return new ShooterSetpoint(rpm, angle);
  }

  public record ShooterSetpoint(double rpm, double angleDeg) {}
}