From e53ffc24bd9cf42bd4f46ebb0690984de183e1c8 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Mon, 12 Feb 2024 17:13:31 -0800 Subject: [PATCH] Feat: added lookup tables and functions to launch at distance and angle --- src/main/java/frc/robot/InterpLUT.java | 145 ++++++++++++++++++ .../robot/subsystems/ShooterSubsystem.java | 48 ++++++ 2 files changed, 193 insertions(+) create mode 100644 src/main/java/frc/robot/InterpLUT.java diff --git a/src/main/java/frc/robot/InterpLUT.java b/src/main/java/frc/robot/InterpLUT.java new file mode 100644 index 0000000..a295648 --- /dev/null +++ b/src/main/java/frc/robot/InterpLUT.java @@ -0,0 +1,145 @@ +package frc.robot; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +/** + * Performs spline interpolation given a set of control points. + */ +public class InterpLUT { + + private List mX = new ArrayList<>(); + private List mY = new ArrayList<>(); + private List mM = new ArrayList<>(); + + private InterpLUT(List x, List y, List m) { + mX = x; + mY = y; + mM = m; + } + + public InterpLUT() { + } + + public void add(double input, double output) { + mX.add(input); + mY.add(output); + } + + /** + * Creates a monotone cubic spline from a given set of control points. + * + *

+ * The spline is guaranteed to pass through each control point exactly. Moreover, assuming the control points are + * monotonic (Y is non-decreasing or non-increasing) then the interpolated values will also be monotonic. + * + * @throws IllegalArgumentException if the X or Y arrays are null, have different lengths or have fewer than 2 values. + */ + //public static LUTWithInterpolator createLUT(List x, List y) { + public void createLUT() { + List x = this.mX; + List y = this.mY; + + if (x == null || y == null || x.size() != y.size() || x.size() < 2) { + throw new IllegalArgumentException("There must be at least two control " + + "points and the arrays must be of equal length."); + } + + final int n = x.size(); + Double[] d = new Double[n - 1]; // could optimize this out + Double[] m = new Double[n]; + + // Compute slopes of secant lines between successive points. + for (int i = 0; i < n - 1; i++) { + Double h = x.get(i + 1) - x.get(i); + if (h <= 0f) { + throw new IllegalArgumentException("The control points must all " + + "have strictly increasing X values."); + } + d[i] = (y.get(i + 1) - y.get(i)) / h; + } + + // Initialize the tangents as the average of the secants. + m[0] = d[0]; + for (int i = 1; i < n - 1; i++) { + m[i] = (d[i - 1] + d[i]) * 0.5f; + } + m[n - 1] = d[n - 2]; + + // Update the tangents to preserve monotonicity. + for (int i = 0; i < n - 1; i++) { + if (d[i] == 0f) { // successive Y values are equal + m[i] = Double.valueOf(0f); + m[i + 1] = Double.valueOf(0f); + } else { + double a = m[i] / d[i]; + double b = m[i + 1] / d[i]; + double h = Math.hypot(a, b); + if (h > 9f) { + double t = 3f / h; + m[i] = t * a * d[i]; + m[i + 1] = t * b * d[i]; + } + } + } + mX = x; + mY = y; + mM = Arrays.asList(m); + } + + /** + * Interpolates the value of Y = f(X) for given X. Clamps X to the domain of the spline. + * + * @param input The X value. + * @return The interpolated Y = f(X) value. + */ + public double get(double input) { + // Handle the boundary cases. + final int n = mX.size(); + if (Double.isNaN(input)) { + return input; + } + if (input <= mX.get(0)) { + throw new IllegalArgumentException("User requested value outside of bounds of LUT. Bounds are: " + mX.get(0).toString() + " to " + mX.get(n - 1).toString() + ". Value provided was: " + input); + } + if (input >= mX.get(n - 1)) { + throw new IllegalArgumentException("User requested value outside of bounds of LUT. Bounds are: " + mX.get(0).toString() + " to " + mX.get(n - 1).toString() + ". Value provided was: " + input); + } + + // Find the index 'i' of the last point with smaller X. + // We know this will be within the spline due to the boundary tests. + int i = 0; + while (input >= mX.get(i + 1)) { + i += 1; + if (input == mX.get(i)) { + return mY.get(i); + } + } + + // Perform cubic Hermite spline interpolation. + double h = mX.get(i + 1) - mX.get(i); + double t = (input - mX.get(i)) / h; + return (mY.get(i) * (1 + 2 * t) + h * mM.get(i) * t) * (1 - t) * (1 - t) + + (mY.get(i + 1) * (3 - 2 * t) + h * mM.get(i + 1) * (t - 1)) * t * t; + } + + // For debugging. + @Override + public String toString() { + StringBuilder str = new StringBuilder(); + final int n = mX.size(); + str.append("["); + for (int i = 0; i < n; i++) { + if (i != 0) { + str.append(", "); + } + str.append("(").append(mX.get(i)); + str.append(", ").append(mY.get(i)); + str.append(": ").append(mM.get(i)).append(")"); + } + str.append("]"); + return str.toString(); + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 3b16906..83ee379 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.InterpLUT; public class ShooterSubsystem extends SubsystemBase { /** Creates a new ShooterSubsystem. */ private CANSparkFlex m_bottom = new CANSparkFlex(20, MotorType.kBrushless); @@ -22,6 +23,12 @@ public class ShooterSubsystem extends SubsystemBase { private RelativeEncoder m_bottomEncoder; private double top_spd = 0; private double bottom_spd = 0; + //input: the angle + //output: the ratio between top/bottom motors + InterpLUT angleLUT = new InterpLUT(); + //input: distance + //output: motor power + InterpLUT distanceLUT = new InterpLUT(); public ShooterSubsystem() { m_topEncoder = m_top.getEncoder(); @@ -32,6 +39,32 @@ public void spin(double speed){ m_topPID.setSetpoint(speed); m_bottomPID.setSetpoint(speed); } + public void spin(double t_speed, double b_speed) { + m_topPID.setSetpoint(t_speed); + m_bottomPID.setSetpoint(b_speed); + } + public void spinDifference(double power, double ratio) { + double t_spd = power*ratio; + double b_spd = power/ratio; + m_topPID.setSetpoint(t_spd); + m_bottomPID.setSetpoint(b_spd); + } + /** + * + * @param power + * @param angle from straight forward, (idk) is shooting straight + */ + public void shoot(double power, double angle) { + spinDifference(power, angleLUT.get(angle)); + } + /** + * + * @param distance in inches + */ + public void shootDistance(double distance) { + spin(distanceLUT.get(distance)); + } + @Override public void periodic() { @@ -46,4 +79,19 @@ public void periodic() { //SmartDashboard.putNumber("ShooterSpeedat_top", top_spd); //SmartDashboard.putNumber("ShooterSpeedat_bottom", bottom_spd); } + public void setUpLUTs() { + + //TODO: Add actual non-dummy values + angleLUT.add(0, 1); + angleLUT.add(-10, 0.7); + angleLUT.add(10, 1.42); + angleLUT.createLUT(); + + distanceLUT.add(0, 0); + distanceLUT.add(1, 0.01); + distanceLUT.add(10, 0.1); + distanceLUT.add(20, 0.25); + distanceLUT.add(50, 0.7); + distanceLUT.createLUT(); + } }