Enhance HoodedShooterSupersystem with ShooterDataPoint class and interpolation logic

Author: Tortuga-AM

src/main/java/frc/robot/Subsystems/HoodedShooterSupersystem/HoodedShooterSupersystem.java

@@ -1,22 +1,50 @@
 package frc.robot.Subsystems.HoodedShooterSupersystem;
 
 import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
 import static frc.robot.Subsystems.HoodedShooterSupersystem.HoodedShooterSupersystemConstants.*;
-
-import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+import java.util.List;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Distance;
 import frc.robot.Subsystems.AdjustableHood.AdjustableHood;
 import frc.robot.Subsystems.Shooter.Shooter;
 import org.littletonrobotics.junction.Logger;
 import org.team7525.subsystem.Subsystem;
 
+class ShooterDataPoint {
+	private AngularVelocity speed;
+	private Angle angle;
+	private Distance distance;
+
+	public ShooterDataPoint(AngularVelocity speed, Angle angle, Distance distance) {
+		this.speed = speed;
+		this.angle = angle;
+		this.distance = distance;
+	}
+
+	public AngularVelocity getSpeed() {
+		return speed;
+	}
+
+	public Angle getAngle() {
+		return angle;
+	}
+
+	public Distance getDistance() {
+		return distance;
+	}
+}
+
 public class HoodedShooterSupersystem extends Subsystem<HoodedShooterSupersystemStates> {
 
 	private static HoodedShooterSupersystem instance;
 	private AdjustableHood hood;
 	private Shooter shooter;
-	private InterpolatingDoubleTreeMap hoodCalculator;
+	private int dataLength;
+	private List<ShooterDataPoint> shots;
+	private ShooterDataPoint currentShot;
 
 	public static HoodedShooterSupersystem getInstance() {
 		if (instance == null) {
@@ -29,16 +57,18 @@ private HoodedShooterSupersystem() {
 		super(SUBSYSTEM_NAME, HoodedShooterSupersystemStates.IDLE);
 		hood = AdjustableHood.getInstance();
 		shooter = Shooter.getInstance();
-		hoodCalculator = new InterpolatingDoubleTreeMap();
-		for (int i = 0; i < ANGLES.length; i++) {
-			hoodCalculator.put(DISTANCES[i], ANGLES[i]);
-		}
+		dataLength = SHOOTER_DATA_POINTS.size() - 1;
+		shots = SHOOTER_DATA_POINTS;
+		currentShot = null;
 	}
 
 	@Override
 	public void runState() {
 		hood.setState(getState().getAdjustableHoodState());
 		shooter.setState(getState().getShooterState());
+		if (getState() == HoodedShooterSupersystemStates.DYNAMIC){
+			currentShot = interpolateShotValues(Limelight.getInstance().getDistanceToTarget());
+		}
 
 		hood.periodic();
 		shooter.periodic();
@@ -55,12 +85,29 @@ public boolean readyToShoot() {
 		return hood.atSetpoint() && shooter.atSetpoint();
 	}
 
-	public Angle calculateHoodSetpoint() {
-		// ZERO is placeholder for logic to get distance from the target.
-		return Degrees.of(hoodCalculator.get((double) 0));
-	}
+	public ShooterDataPoint interpolateShotValues(Distance targetDistance) {
+		shots.sort((a, b) -> a.getDistance().compareTo(b.getDistance()));
+		for (int i = 0; i < dataLength; i++) {
+			ShooterDataPoint s1 = shots.get(i);
+			ShooterDataPoint s2 = shots.get(i + 1);
 
-	public AngularVelocity calculateShooterSetpoint() {
-		return MEDIUM_SPEED;
+			if (targetDistance.in(Meters) >= s1.getDistance().in(Meters) && targetDistance.in(Meters) <= s2.getDistance().in(Meters)) {
+				
+				double ratio = (targetDistance.in(Meters) - s1.getDistance().in(Meters)) / 
+					(s2.getDistance().in(Meters) - s1.getDistance().in(Meters));
+				// Interpolate speed
+				double interpolatedSpeedValue = s1.getDistance().in(Meters)
+						+ ratio * (s1.getSpeed().in(RotationsPerSecond) - s1.getSpeed().in(RotationsPerSecond));
+				AngularVelocity interpolatedSpeed = RotationsPerSecond.of(interpolatedSpeedValue);
+				// Interpolate angle
+				double interpolatedAngleValue = s1.getAngle().in(Degrees)
+						+ ratio * (s2.getAngle().in(Degrees) - s1.getAngle().in(Degrees));
+				Angle interpolatedAngle = Degrees.of(interpolatedAngleValue);
+
+				// Set states
+				return new ShooterDataPoint(interpolatedSpeed, interpolatedAngle, targetDistance); // Successful interpolation
+			}
+		}
+				return null;
 	}
 }

src/main/java/frc/robot/Subsystems/HoodedShooterSupersystem/HoodedShooterSupersystemConstants.java

@@ -1,6 +1,9 @@
 package frc.robot.Subsystems.HoodedShooterSupersystem;
 
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Meters;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
+import java.util.List;
 
 import edu.wpi.first.units.measure.AngularVelocity;
 
@@ -14,6 +17,10 @@ public class HoodedShooterSupersystemConstants {
 	public static final AngularVelocity MEDIUM_SPEED = RotationsPerSecond.of(1500);
 
 	// Random values and stuff
-	public static final double[] ANGLES = { 0, 10, 20, 30, 40, 50, 60, 70, 80, 90 };
-	public static final double[] DISTANCES = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9 };
+	public static final List<ShooterDataPoint> SHOOTER_DATA_POINTS = List.of(
+		new ShooterDataPoint(RotationsPerSecond.of(1500), Degrees.of(30), Meters.of(2.0)),
+		new ShooterDataPoint(RotationsPerSecond.of(2000), Degrees.of(35), Meters.of(3.0)),
+		new ShooterDataPoint(RotationsPerSecond.of(2500), Degrees.of(40), Meters.of(3.5)),
+		new ShooterDataPoint(RotationsPerSecond.of(3000), Degrees.of(45), Meters.of(4.0))
+	);
 }

vendordeps/maple-sim.json

@@ -1,7 +1,7 @@
 {
     "fileName": "maple-sim.json",
     "name": "maplesim",
-    "version": "0.3.11",
+    "version": "0.3.14",
     "frcYear": "2025",
     "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b",
     "mavenUrls": [
@@ -13,7 +13,7 @@
         {
             "groupId": "org.ironmaple",
             "artifactId": "maplesim-java",
-            "version": "0.3.11"
+            "version": "0.3.14"
         },
         {
             "groupId": "org.dyn4j",