From: Arnav495 Date: Fri, 6 Feb 2026 00:12:47 +0000 (-0800) Subject: Add constraints to shooter code. X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=2b5ed7f3a8bba859191e4ed331580a3cb8366125;p=FRC2026.git Add constraints to shooter code. --- diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java index d66eadc..40fec43 100644 --- a/src/main/java/frc/robot/util/ShooterPhysics.java +++ b/src/main/java/frc/robot/util/ShooterPhysics.java @@ -2,15 +2,29 @@ package frc.robot.util; import java.util.Optional; +import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; import frc.robot.constants.Constants; public class ShooterPhysics { // pitch in radians, going up from the horizontal // exit velocity speed in m/s - public record TurretState(Rotation2d yaw, double pitch, double exitVel) { + public record TurretState(Rotation2d yaw, double pitch, double exitVel, double height) { + public boolean satisfies(Constraints constraints) { + if (height < constraints.height()) + return false; + if (exitVel > constraints.maxVel()) + return false; + if (pitch > constraints.maxPitch() || pitch < constraints.minPitch()) + return false; + return true; + } + }; + + public record Constraints(double height, double maxVel, double minPitch, double maxPitch) { }; /** @@ -25,37 +39,82 @@ public class ShooterPhysics { public static TurretState getShotParams(Translation2d robotVelocity, Translation3d robotToTarget, double height) { Translation3d exitVel = getRequiredExitVelocity(robotVelocity, robotToTarget, height); + return cvtShot(exitVel, height); + } - Translation2d onGround = exitVel.toTranslation2d(); - Rotation2d yaw = onGround.getAngle(); - double magnitude2d = onGround.getNorm(); + public static Optional getConstrainedParams(Translation2d robotVelocity, Translation3d robotToTarget, + Constraints constraints) { + // establish a lower bound + double minHeight = Math.max(robotToTarget.getZ(), constraints.height()); + Optional withMinPitch = withAngle(robotVelocity, robotToTarget, constraints.minPitch()); + if (withMinPitch.isPresent()) { + minHeight = Math.min(minHeight, withMinPitch.get().height()); + } + TurretState withMinHeight = cvtShot(getRequiredExitVelocity(robotVelocity, robotToTarget, minHeight), + minHeight); + if (withMinHeight.satisfies(constraints)) + return Optional.of(withMinHeight); - double pitch = new Translation2d(magnitude2d, exitVel.getZ()).getAngle().getRadians(); - pitch %= Math.PI * 2; - double speed = exitVel.getDistance(Translation3d.kZero); + // the only reason this is empty is if the highest posible trajectory is too low + // to intersect the goal + Optional withMaxPitchOpt = withAngle(robotVelocity, robotToTarget, constraints.minPitch()); + if (withMaxPitchOpt.isEmpty()) + return Optional.empty(); + TurretState withMaxPitch = withMaxPitchOpt.get(); - return new TurretState(yaw, pitch, speed); - } + // the range from withMinHeight to withMaxPitch will satisfy pitch and height + // constraints + // now we need to satisfy the speed constraint - public static Optional getShotParamsBySpeed(Translation2d robotVelocity, Translation2d robot, - Translation3d target, - double speed) { - Translation3d robotToTarget = target.minus(new Translation3d(robot)); - Optional exitVelOpt = getExitVelocityForSpeed(robotVelocity, robotToTarget, speed); + TurretState withMinSpeed = withMinimumSpeed(robotVelocity, robotToTarget); + // ordered such that the first element is valid and the second is not + Pair newRange; - if (exitVelOpt.isEmpty()) + if (withMinSpeed.height() < withMinHeight.height()) { + // the minimum speed is below the lower bound, but doesn't satisfy constraints return Optional.empty(); - Translation3d exitVel = exitVelOpt.get(); - Translation2d onGround = exitVel.toTranslation2d(); + } else if (withMinSpeed.height() > withMaxPitch.height()) { + // the minimum speed is above the upper bound + if (withMaxPitch.satisfies(constraints)) + // keep optimizing to find the lowest height + newRange = new Pair(withMaxPitch, withMinHeight); + else + return Optional.empty(); + + } else { + // the minimum speed is within the ok range + assert withMinSpeed.satisfies(constraints); + newRange = new Pair(withMinSpeed, withMinHeight); + } + + // now we binary search the new range + TurretState lastValid = newRange.getFirst(); + // use a 5cm tolerance + while (Math.abs(newRange.getFirst().height() - newRange.getSecond().height()) < .05) { + double avgHeight = (newRange.getFirst().height() + newRange.getSecond().height()) / 2; + TurretState guess = cvtShot(getRequiredExitVelocity(robotVelocity, robotToTarget, avgHeight), avgHeight); + if (guess.satisfies(constraints)) { + if (guess.height() < lastValid.height()) lastValid = guess; + newRange = new Pair(guess, newRange.getSecond()); + } else { + newRange = new Pair(newRange.getFirst(), guess); + } + } + + return Optional.of(lastValid); + } + + public static TurretState cvtShot(Translation3d velocity, double height) { + Translation2d onGround = velocity.toTranslation2d(); Rotation2d yaw = onGround.getAngle(); double magnitude2d = onGround.getNorm(); - double pitch = new Translation2d(magnitude2d, exitVel.getZ()).getAngle().getRadians(); + double pitch = new Translation2d(magnitude2d, velocity.getZ()).getAngle().getRadians(); pitch %= Math.PI * 2; - double speed2 = exitVel.getDistance(Translation3d.kZero); + double speed = velocity.getDistance(Translation3d.kZero); - return Optional.of(new TurretState(yaw, pitch, speed2)); + return new TurretState(yaw, pitch, speed, height); } /** @@ -114,13 +173,12 @@ public class ShooterPhysics { } // call with default tolerance - public static Optional getExitVelocityForSpeed(Translation2d initialVelocity, Translation3d target, - double speed) { - return getExitVelocityForSpeed(initialVelocity, target, speed, 0.1); + public static TurretState withMinimumSpeed(Translation2d initialVelocity, Translation3d target) { + return withMinimumSpeed(initialVelocity, target, 0.1); } - public static Optional getExitVelocityForSpeed(Translation2d initialVelocity, Translation3d target, - double speed, double tolerance) { + public static TurretState withMinimumSpeed(Translation2d initialVelocity, Translation3d target, + double tolerance) { // calculate minimum velocity: v² = g * (R + √(R² + h²)) double horizontalDist = target.toTranslation2d().getNorm(); @@ -132,10 +190,42 @@ public class ShooterPhysics { .sqrt(g * (horizontalDist + Math.sqrt(horizontalDist * horizontalDist + verticalDist * verticalDist))); double minSpeed = Math.max(0, minProjectileSpeed - robotSpeed); - if (speed < minSpeed - tolerance) { - return Optional.empty(); + // guess a peak height + double guess = target.getZ() + 2; + int maxIters = 20; + while (maxIters >= 0) { + maxIters--; + + // this will throw an exception, so avoid it + // we still might have just overshot, so keep checking + if (guess < target.getZ()) + guess = target.getZ(); + + Translation3d guessVelocity = getRequiredExitVelocity(initialVelocity, target, guess); + double guessSpeed = guessVelocity.getNorm(); + double difference = minSpeed - guessSpeed; + + // we've already hit minimum height and are trying to go lower + if (guess <= target.getZ() && difference < 0) + throw new RuntimeException("Incorrect minimum speed calculation in ShooterPhysics.java"); + + if (Math.abs(difference) <= tolerance) + return cvtShot(guessVelocity, guess); + + guess += difference * 1.7; // experimentally determined value } + throw new RuntimeException("Failed to compute a trajectory for a minimum speed."); + } + + public static Optional withAngle(Translation2d initialVelocity, Translation3d target, + double pitch) { + return withAngle(initialVelocity, target, pitch, Units.degreesToRadians(1)); + } + + public static Optional withAngle(Translation2d initialVelocity, Translation3d target, + double pitch, double tolerance) { + // guess a peak height double guess = target.getZ() + 2; int maxIters = 20; @@ -148,19 +238,19 @@ public class ShooterPhysics { guess = target.getZ(); Translation3d guessVelocity = getRequiredExitVelocity(initialVelocity, target, guess); - double guessSpeed = guessVelocity.getNorm(); - double difference = speed - guessSpeed; + TurretState polar = cvtShot(guessVelocity, guess); + double difference = pitch - polar.pitch(); // we've already hit minimum height and are trying to go lower if (guess <= target.getZ() && difference < 0) return Optional.empty(); if (Math.abs(difference) <= tolerance) - return Optional.of(guessVelocity); + return Optional.of(polar); - guess += difference * 1.7; // experimentally determined value + guess += difference * 0.7; // TODO: find better value } - return Optional.empty(); + throw new RuntimeException("Solving for angle did not converge."); } } diff --git a/src/test/java/frc/robot/util/ShooterPhysicsTest.java b/src/test/java/frc/robot/util/ShooterPhysicsTest.java index 8dd3a53..c809896 100644 --- a/src/test/java/frc/robot/util/ShooterPhysicsTest.java +++ b/src/test/java/frc/robot/util/ShooterPhysicsTest.java @@ -5,7 +5,6 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; import java.util.ArrayList; -import java.util.Optional; import java.util.Random; import org.junit.jupiter.api.AfterEach; @@ -131,20 +130,6 @@ class ShooterPhysicsTest { assertTrue(state6.pitch() >= 0 && state6.pitch() <= Math.PI / 16, state6.toString()); } - @Test - public void velocityTest() { - // just make sure it converges properly - Optional translation = ShooterPhysics.getExitVelocityForSpeed(new Translation2d(4.2, -3), - new Translation3d(1, 2, 3), 20); - assertTrue(translation.isPresent()); - assertEquals(translation.get().getNorm(), 20, 0.1); - - // check something impossible is impossible - Optional translation2 = ShooterPhysics.getExitVelocityForSpeed(Translation2d.kZero, - new Translation3d(100, 0, 5), 10); - assertTrue(translation2.isEmpty()); - } - // test using a simple physics simulation @Test public void simulatedTest() {