From 9a25c397f078eaf1809c2eb6f5447b7bbce78cfa Mon Sep 17 00:00:00 2001 From: Ahluwalia Date: Fri, 27 Feb 2026 21:05:16 -0800 Subject: [PATCH] Made changes. --- .../robot/commands/gpm/AutoShootCommand.java | 7 +- .../commands/gpm/IntakeMovementCommand.java | 4 +- .../robot/commands/gpm/Superstructure.java | 8 +- .../robot/constants/ShotInterpolation.java | 6 + .../frc/robot/subsystems/shooter/Shooter.java | 2 +- .../frc/robot/subsystems/turret/Turret.java | 2 +- .../java/frc/robot/util/ShooterPhysics.java | 311 ++++++++---------- .../frc/robot/util/ShooterPhysicsTest.java | 180 +--------- 8 files changed, 171 insertions(+), 349 deletions(-) diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java index 14583ba..3aa1ee9 100644 --- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java +++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java @@ -1,5 +1,7 @@ package frc.robot.commands.gpm; +import static edu.wpi.first.units.Units.Gs; + import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.MathUtil; @@ -108,13 +110,12 @@ public class AutoShootCommand extends Command { for (int i = 0; i < 20; i++) { Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(), TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ()); Translation3d target3d = new Translation3d(target.getX(), target.getY(), FieldConstants.getHubTranslation().getZ()); - var goalStateWithT = ShooterPhysics.getShotParamsWithT( + goalState = ShooterPhysics.getShotParams( Translation2d.kZero, target3d.minus(lookahead3d), 2.0); - goalState = goalStateWithT.getFirst(); - timeOfFlight = goalStateWithT.getSecond(); + timeOfFlight = goalState.timeOfFlight(); double offsetX = turretVelocityX * timeOfFlight; double offsetY = turretVelocityY * timeOfFlight; lookaheadPose = diff --git a/src/main/java/frc/robot/commands/gpm/IntakeMovementCommand.java b/src/main/java/frc/robot/commands/gpm/IntakeMovementCommand.java index 2365a9c..a623e61 100644 --- a/src/main/java/frc/robot/commands/gpm/IntakeMovementCommand.java +++ b/src/main/java/frc/robot/commands/gpm/IntakeMovementCommand.java @@ -15,7 +15,7 @@ public class IntakeMovementCommand extends Command { @Override public void execute() { - intake.spinStop(); + intake.spinStart(); if ((int) (Timer.getFPGATimestamp() / interval) % 2 == 0) { intake.extend(); } else { @@ -25,6 +25,6 @@ public class IntakeMovementCommand extends Command { @Override public void end(boolean interrupted) { - + intake.extend(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java index 7e55227..e23f90c 100644 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -119,16 +119,14 @@ public class Superstructure extends Command { target == FieldConstants.getHubTranslation().toTranslation2d() ? FieldConstants.getHubTranslation().getZ() : 0.0); // Height of 0 if it's not the hub - var goalStateWithT = ShooterPhysics.getShotParamsWithT( + goalState = ShooterPhysics.getShotParams( Translation2d.kZero, target3d.minus(lookahead3d), target == FieldConstants.getHubTranslation().toTranslation2d() ? 2.0 : 5.0); - - goalState = goalStateWithT.getFirst(); double TOFAdjustment = 0.75; - timeOfFlight = goalStateWithT.getSecond() * TOFAdjustment; + timeOfFlight = goalState.timeOfFlight() * TOFAdjustment; double offsetX = turretVelocityX * timeOfFlight; double offsetY = turretVelocityY * timeOfFlight; lookaheadPose = @@ -223,7 +221,7 @@ public class Superstructure extends Command { turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint), turretVelocity - drivetrain.getAngularRate(2)); if(phaseManager.getCurrentState() == CurrentState.UNDER_TRENCH){ - hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE - hoodOffset), 0.0); + hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0); } else{ hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.hoodAngleMap.get(hoodSetpoint)), hoodVelocity); } diff --git a/src/main/java/frc/robot/constants/ShotInterpolation.java b/src/main/java/frc/robot/constants/ShotInterpolation.java index da32329..4f8ea0d 100644 --- a/src/main/java/frc/robot/constants/ShotInterpolation.java +++ b/src/main/java/frc/robot/constants/ShotInterpolation.java @@ -33,6 +33,12 @@ public class ShotInterpolation { exitVelocityMap.put(7.9, 17.1); exitVelocityMap.put(8.0, 17.9); exitVelocityMap.put(8.08, 19.0); + exitVelocityMap.put( 21.0, 19.0); + + exitVelocityMap.put(9.90, 14.0); + exitVelocityMap.put(9.95, 16.5); + exitVelocityMap.put(10.0, 19.2); + exitVelocityMap.put(11.0, 26.0); exitVelocityMap.put(25.0, 25.0* 3.2); //exitVelocityMap.put(null, null); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 9b1a45a..ebe389f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -30,7 +30,7 @@ public class Shooter extends SubsystemBase implements ShooterIO { private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); - double powerModifier = 1.0; + double powerModifier = .75; public Shooter(){ updateInputs(); diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 15f13af..3a83e8d 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -77,7 +77,7 @@ public class Turret extends SubsystemBase implements TurretIO{ config.Slot0.kP = 12.0; config.Slot0.kS = 0.1; // Static friction compensation config.Slot0.kV = 0.12; // Adjusted kV for the gear ratio - config.Slot0.kD = 0.50; // The "Braking" term to stop overshoot + config.Slot0.kD = 0.2; // The "Braking" term to stop overshoot var mm = config.MotionMagic; mm.MotionMagicCruiseVelocity = Units.radiansToRotations(TurretConstants.MAX_VELOCITY) * TurretConstants.GEAR_RATIO; diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java index 0575981..4e40995 100644 --- a/src/main/java/frc/robot/util/ShooterPhysics.java +++ b/src/main/java/frc/robot/util/ShooterPhysics.java @@ -12,7 +12,7 @@ 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, double height) { + public record TurretState(Rotation2d yaw, double pitch, double exitVel, double height, double timeOfFlight) { public boolean satisfies(Constraints constraints) { if (height < constraints.height()) return false; @@ -24,126 +24,131 @@ public class ShooterPhysics { } }; - // note: minPitch must be < 75° + /** + * Specifies the constraints of the shot. + * + * @param height The minimum apex height of the shot. + * @param maxVel The maximum exit velocity of the shooter + * @param minPitch The minimum pitch covered by the hood + * @param maxPitch The maximum pitch covered by the hood + */ public record Constraints(double height, double maxVel, double minPitch, double maxPitch) { - // performs some sanity checks - public boolean check() { - if (height <= 0) - return false; - if (maxVel <= 0) - return false; - if (minPitch <= 0) - return false; - if (maxPitch >= Math.PI / 2) - return false; - if (minPitch >= maxPitch) - return false; - - // this causes problems and isn't likely to actually occur - if (minPitch > Units.degreesToRadians(75)) - return false; - - return true; - } }; /** - * Calculates shot parameters for SOTM for a peak height using Physics™. + * Calculates shot parameters for SOTM using Physics™. * * @param robotVelocity The x and y velocity of the robot, field relative. * @param robotToTarget The robot to target transform. Angles are field * relative, positions are with the robot as the origin. - * @param height The peak height the trajectory should reach. + * @param peakHeight The peak height the trajectory should reach. * @return A TurretState that represents the shot the robot should take. */ public static TurretState getShotParams(Translation2d robotVelocity, Translation3d robotToTarget, - double height) { - Translation3d exitVel = getRequiredExitVelocity(robotVelocity, robotToTarget, height); - return cvtShot(exitVel, height); - } - - // provided for backwards compatability - public static Pair getShotParamsWithT(Translation2d robotVelocity, Translation3d robotToTarget, - double height) { - Translation3d exitVel = getRequiredExitVelocity(robotVelocity, robotToTarget, height); - double t = (-exitVel.getZ() - - Math.sqrt(Math.pow(exitVel.getZ(), 2) - 2 * Constants.GRAVITY_ACCELERATION * robotToTarget.getZ())) - / -Constants.GRAVITY_ACCELERATION; - return new Pair(cvtShot(exitVel, height), t); + double peakHeight) { + double zExitVel = Math.sqrt(2 * peakHeight * Constants.GRAVITY_ACCELERATION); + double t = (-zExitVel - Math.sqrt(Math.pow(zExitVel, 2) - 2 * Constants.GRAVITY_ACCELERATION * robotToTarget.getZ())) / -Constants.GRAVITY_ACCELERATION; + Translation3d exitVel = getRequiredExitVelocity(robotVelocity, robotToTarget, peakHeight); + return cvtShot(exitVel, peakHeight, t); } /** - * Calculates shot parameters for SOTM for a peak height using Physics™. + * Calculates shot parameters for stationary shooting using Physics™. * - * @param robotVelocity The x and y velocity of the robot, field relative. * @param robotToTarget The robot to target transform. Angles are field * relative, positions are with the robot as the origin. - * @param constraints The constraints on the shooter. + * @param peakHeight The peak height the trajectory should reach. * @return A TurretState that represents the shot the robot should take. */ + public static TurretState getShotParams(Translation3d robotToTarget, double peakHeight) { + return getShotParams(new Translation2d(0, 0), robotToTarget, peakHeight); + } + + public static Optional getConstrainedParams(Translation3d robotToTarget, Constraints constraints){ + return getConstrainedParams(new Translation2d(0, 0), robotToTarget, constraints); + } + public static Optional getConstrainedParams(Translation2d robotVelocity, Translation3d robotToTarget, Constraints constraints) { - if (!constraints.check()) - throw new IllegalArgumentException("Provided constraints are invalid (" + constraints + ")."); - // establish a lower bound - double minHeight = Math.max(Math.max(robotToTarget.getZ(), constraints.height()), 0.01); + double minHeight = Math.max(robotToTarget.getZ(), constraints.height()); Optional withMinPitch = withAngle(robotVelocity, robotToTarget, constraints.minPitch()); - if (withMinPitch.isPresent()) - minHeight = Math.max(minHeight, withMinPitch.get().height()); - - TurretState withMinHeight = getShotParams(robotVelocity, robotToTarget, minHeight); + if (withMinPitch.isPresent()) { + minHeight = Math.min(minHeight, withMinPitch.get().height()); + } + Translation3d minVel = getRequiredExitVelocity(robotVelocity, robotToTarget, minHeight); + double minT = (-minVel.getZ() - Math.sqrt(Math.pow(minVel.getZ(), 2) - 2 * Constants.GRAVITY_ACCELERATION * robotToTarget.getZ())) / -Constants.GRAVITY_ACCELERATION; + TurretState withMinHeight = cvtShot(minVel, minHeight, minT); if (withMinHeight.satisfies(constraints)) return Optional.of(withMinHeight); - TurretState withMinSpeed = withMinimumSpeed(robotVelocity, robotToTarget); - if (withMinSpeed.exitVel() > constraints.maxVel()) + // 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(); + + // the range from withMinHeight to withMaxPitch will satisfy pitch and height + // constraints + // now we need to satisfy the speed constraint + + TurretState withMinSpeed = withMinimumSpeed(robotVelocity, robotToTarget); + // ordered such that the first element is valid and the second is not + Pair newRange; if (withMinSpeed.height() < withMinHeight.height()) { // the minimum speed is below the lower bound, but doesn't satisfy constraints return Optional.empty(); - } else { - // the first element is lower than the second, the first satisfies the angle but - // not the velocity and the second satisfies the velocity but may or may not - // satisfy the angle - var newRange = new Pair(withMinHeight, withMinSpeed); - - // now we binary search the new range to find the lowest value that satisfies - // the velocity constraint, we know velocity is decreasing on the interval - - // use a 1cm tolerance - while (Math.abs(newRange.getFirst().height() - newRange.getSecond().height()) > .01) { - double avgHeight = (newRange.getFirst().height() + newRange.getSecond().height()) / 2; - TurretState guess = getShotParams(robotVelocity, robotToTarget, avgHeight); - if (guess.exitVel() > constraints.maxVel()) - newRange = new Pair(guess, newRange.getSecond()); - else - newRange = new Pair(newRange.getFirst(), guess); - - } - if (newRange.getSecond().satisfies(constraints)) - return Optional.of(newRange.getSecond()); + } 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; + Translation3d guessVel = getRequiredExitVelocity(robotVelocity, robotToTarget, avgHeight); + double guessT = (-guessVel.getZ() - Math.sqrt(Math.pow(guessVel.getZ(), 2) - 2 * Constants.GRAVITY_ACCELERATION * robotToTarget.getZ())) / -Constants.GRAVITY_ACCELERATION; + TurretState guess = cvtShot(guessVel, avgHeight, guessT); + 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); } - static TurretState cvtShot(Translation3d velocity, double height) { + public static TurretState cvtShot(Translation3d velocity, double height, double timeOfFlight) { Translation2d onGround = velocity.toTranslation2d(); Rotation2d yaw = onGround.getAngle(); double magnitude2d = onGround.getNorm(); double pitch = new Translation2d(magnitude2d, velocity.getZ()).getAngle().getRadians(); + pitch %= Math.PI * 2; double speed = velocity.getDistance(Translation3d.kZero); - return new TurretState(yaw, pitch, speed, height); + return new TurretState(yaw, pitch, speed, height, timeOfFlight); } /** * Actually does the SOTM math. Assumes shots are from (0, 0, 0). This is only - * package scope so it can be unit tested, and shouldn't be called directly. + * public so it can be unit tested, and shouldn't be called directly. * * @param robotVelocity The velocity of the robot, field relative. * @param target The translation from the robot to the target, field @@ -153,7 +158,7 @@ public class ShooterPhysics { * trajectory. * @return Velocity that should be imparted on the ball, field relative. */ - static Translation3d getRequiredExitVelocity(Translation2d robotVelocity, Translation3d target, + public static Translation3d getRequiredExitVelocity(Translation2d robotVelocity, Translation3d target, double peakZ) { if (target.getZ() > peakZ) throw new IllegalArgumentException( @@ -168,10 +173,7 @@ public class ShooterPhysics { // peakZ = .5 * v_z_exit_vel² / g // v_z_exit_vel² = 2 * peakZ * g // v_z_exit_vel = √(2 * peakZ * g) - // keep a second variable to avoid floating point precision issues where the - // sqrt for t ends up being NaN sometimes when peakZ = target.getZ() - double zExitVelSquared = 2 * peakZ * Constants.GRAVITY_ACCELERATION; - double zExitVel = Math.sqrt(zExitVelSquared); + double zExitVel = Math.sqrt(2 * peakZ * Constants.GRAVITY_ACCELERATION); // now we need time to hit target // z_target = v_z_exit_vel * t - .5 * g * t² @@ -182,13 +184,11 @@ public class ShooterPhysics { // t = (-v_z_exit_vel ± √(v_z_exit_vel² - 2 * g * z_target)) / -g // onlz use - because we only want the part where it's coming down, and that // gives the longer time - double t = (-zExitVel - Math.sqrt(zExitVelSquared - 2 * Constants.GRAVITY_ACCELERATION * target.getZ())) + double t = (-zExitVel - Math.sqrt(Math.pow(zExitVel, 2) - 2 * Constants.GRAVITY_ACCELERATION * target.getZ())) / -Constants.GRAVITY_ACCELERATION; - // this is not equivalent to t <= 0 because of NaNs - if (!(t > 0)) - throw new RuntimeException("Time should never be negative (got t=" + t + " with target: " + target - + " and peakZ: " + peakZ + ")."); + if (t < 0) + throw new RuntimeException("Time should never be negative (got t=" + t + ")."); // calculate x and z exit_vel // x = (v_x_robot + v_x_exit_vel) * t @@ -201,124 +201,87 @@ public class ShooterPhysics { return new Translation3d(xExitVel, yExitVel, zExitVel); } - // gets the derivative of velocity with respect to height at shot - private static double getVelocityDiff(TurretState shot, Translation2d initialVelocity, Translation3d target) { - return (getShotParams(initialVelocity, target, shot.height() + .01).exitVel() - shot.exitVel()) / .01; - } - // call with default tolerance - static TurretState withMinimumSpeed(Translation2d initialVelocity, Translation3d target) { - return withMinimumSpeed(initialVelocity, target, 0.001); + public static TurretState withMinimumSpeed(Translation2d initialVelocity, Translation3d target) { + return withMinimumSpeed(initialVelocity, target, 0.1); } - static TurretState withMinimumSpeed(Translation2d initialVelocity, Translation3d target, + public static TurretState withMinimumSpeed(Translation2d initialVelocity, Translation3d target, double tolerance) { - // System.out.println("!!! inv:" + initialVelocity + " tgt:" + target + " tlr:" - // + tolerance); - // trying to calculate a shot for height=0 returns NaN - double effectiveMinHeight = Math.max(target.getZ(), 0.01); - - TurretState first = getShotParams(initialVelocity, target, effectiveMinHeight); - // if the minimum velocity is below our minimum height, that's the closest we - // can get - if (getVelocityDiff(first, initialVelocity, target) >= 0) - return first; - - // if a shot requires going up a kilometer, it's probably not doable - TurretState second = getShotParams(initialVelocity, target, 1000.); - // just return something - if (getVelocityDiff(second, initialVelocity, target) < 0) - return second; - - int maxIters = 50; - var range = new Pair(first, second); + + // calculate minimum velocity: v² = g * (R + √(R² + h²)) + double horizontalDist = target.toTranslation2d().getNorm(); + double verticalDist = target.getZ(); + double g = Constants.GRAVITY_ACCELERATION; + double robotSpeed = initialVelocity.getNorm(); + + double minProjectileSpeed = Math + .sqrt(g * (horizontalDist + Math.sqrt(horizontalDist * horizontalDist + verticalDist * verticalDist))); + double minSpeed = Math.max(0, minProjectileSpeed - robotSpeed); + + // guess a peak height + double guess = target.getZ() + 2; + int maxIters = 20; while (maxIters >= 0) { maxIters--; - assert range.getSecond().height() > range.getFirst().height(); - double guessHeight = (range.getFirst().height() + range.getSecond().height()) / 2; - TurretState guess = getShotParams(initialVelocity, target, guessHeight); - double diff = getVelocityDiff(guess, initialVelocity, target); + // this will throw an exception, so avoid it + // we still might have just overshot, so keep checking + if (guess < target.getZ()) + guess = target.getZ(); - // System.out.println("diff:" + diff + "\t\t" + range); + Translation3d guessVelocity = getRequiredExitVelocity(initialVelocity, target, guess); + double guessT = (-guessVelocity.getZ() - Math.sqrt(Math.pow(guessVelocity.getZ(), 2) - 2 * Constants.GRAVITY_ACCELERATION * target.getZ())) / -Constants.GRAVITY_ACCELERATION; + double guessSpeed = guessVelocity.getNorm(); + double difference = minSpeed - guessSpeed; - if (Math.abs(range.getSecond().exitVel() - range.getFirst().exitVel()) <= tolerance) - return guess; + // 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 (diff > 0) - range = new Pair(range.getFirst(), guess); - else - range = new Pair(guess, range.getSecond()); - } + if (Math.abs(difference) <= tolerance) + return cvtShot(guessVelocity, guess, guessT); - throw new RuntimeException( - "Solving for minumum velocity did not converge (velocity: " + initialVelocity + ", target: " - + target + ", tolerance: " + tolerance + ")."); + guess += difference * 1.7; // experimentally determined value + } + throw new RuntimeException("Failed to compute a trajectory for a minimum speed."); } - static Optional withAngle(Translation2d initialVelocity, Translation3d target, + public static Optional withAngle(Translation2d initialVelocity, Translation3d target, double pitch) { - return withAngle(initialVelocity, target, pitch, 0.001); + return withAngle(initialVelocity, target, pitch, Units.degreesToRadians(1)); } - // note: this behaves badly with high angles - // this isn't a problem when this is called from getConstrainedParams because - // that will only use this method to solve for the lower bound - static Optional withAngle(Translation2d initialVelocity, Translation3d target, + public static Optional withAngle(Translation2d initialVelocity, Translation3d target, double pitch, double tolerance) { - if (pitch <= 0 || pitch >= Math.PI / 2) - throw new IllegalArgumentException("Pitch must be in the range 0 < pitch < pi/2 (got: " + pitch + ")."); - - // System.out.println( - // "Solving for pitch=" + pitch + " with target=" + target + " and - // initialVelocity=" + initialVelocity); - - // trying to calculate a shot for height=0 returns NaN - double effectiveMinHeight = Math.max(target.getZ(), 0.01); - - TurretState first = getShotParams(initialVelocity, target, effectiveMinHeight); - TurretState second = getShotParams(initialVelocity, target, 1000.); - if (first.pitch() > pitch) - if (first.pitch() <= pitch + tolerance) - return Optional.of(first); - else - return Optional.empty(); - else if (second.pitch() < pitch) - return Optional.of(second); // it's close enough - - int maxIters = 50; - var range = new Pair(first, second); + // guess a peak height + double guess = target.getZ() + 2; + int maxIters = 20; while (maxIters >= 0) { maxIters--; - assert range.getSecond().height() > range.getFirst().height(); - double guessHeight = (range.getFirst().height() + range.getSecond().height()) / 2; - TurretState guess = getShotParams(initialVelocity, target, guessHeight); + // this will throw an exception, so avoid it + // we still might have just overshot, so keep checking + if (guess < target.getZ()) + guess = target.getZ(); - // System.out.println(range + "\t\tguess=" + guess); + Translation3d guessVelocity = getRequiredExitVelocity(initialVelocity, target, guess); + double guessT = (-guessVelocity.getZ() - Math.sqrt(Math.pow(guessVelocity.getZ(), 2) - 2 * Constants.GRAVITY_ACCELERATION * target.getZ())) / -Constants.GRAVITY_ACCELERATION; + TurretState polar = cvtShot(guessVelocity, guess, guessT); + double difference = pitch - polar.pitch(); - if (range.getSecond().pitch() - range.getFirst().pitch() <= tolerance) { - // we've found a valid angle - if (Math.abs(range.getFirst().pitch() - pitch) <= tolerance) - return Optional.of(range.getFirst()); - if (Math.abs(range.getSecond().pitch() - pitch) <= tolerance) - return Optional.of(range.getSecond()); + // we've already hit minimum height and are trying to go lower + if (guess <= target.getZ() && difference < 0) + return Optional.empty(); - // we've narrowed the range but haven't found a valid angle - // should be covered by the checks before the loop - throw new RuntimeException( - "Solving for angle resulted in an empty range " + range + " (pitch: " + pitch + ")."); - } + if (Math.abs(difference) <= tolerance) + return Optional.of(polar); - if (guess.pitch() > pitch) - range = new Pair(range.getFirst(), guess); - else - range = new Pair(guess, range.getSecond()); + guess += difference * 0.7; // TODO: find better value } - throw new RuntimeException("Solving for angle did not converge (velocity: " + initialVelocity + ", target: " - + target + ", pitch: " + pitch + ", tolerance: " + tolerance + ")."); + 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 6e082f3..c809896 100644 --- a/src/test/java/frc/robot/util/ShooterPhysicsTest.java +++ b/src/test/java/frc/robot/util/ShooterPhysicsTest.java @@ -14,13 +14,10 @@ import org.junit.jupiter.api.Test; 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; -import frc.robot.util.ShooterPhysics.Constraints; -import frc.robot.util.ShooterPhysics.TurretState; class ShooterPhysicsTest { - private static final double epsilon = .01; + private static final double epsilon = .001; @BeforeEach public void prepare() { @@ -84,21 +81,21 @@ class ShooterPhysicsTest { @Test public void yawTest() { - TurretState state1 = ShooterPhysics.getShotParams(Translation2d.kZero, + ShooterPhysics.TurretState state1 = ShooterPhysics.getShotParams(Translation2d.kZero, new Translation3d(1, 0, 0), 1); // different for this one because it's close to 0, so the angle wraps and // assertEquals can't handle that assertTrue(Math.abs((state1.yaw()).getRadians()) <= epsilon, state1.toString()); - TurretState state2 = ShooterPhysics.getShotParams(Translation2d.kZero, + ShooterPhysics.TurretState state2 = ShooterPhysics.getShotParams(Translation2d.kZero, new Translation3d(0, 1, 0), 1); assertEquals(new Rotation2d(Math.PI / 2).getRadians(), state2.yaw().getRadians(), epsilon); - TurretState state3 = ShooterPhysics.getShotParams(Translation2d.kZero, + ShooterPhysics.TurretState state3 = ShooterPhysics.getShotParams(Translation2d.kZero, new Translation3d(-1, 0, 0), 1); assertEquals(new Rotation2d(Math.PI).getRadians(), state3.yaw().getRadians(), epsilon); - TurretState state4 = ShooterPhysics.getShotParams(Translation2d.kZero, + ShooterPhysics.TurretState state4 = ShooterPhysics.getShotParams(Translation2d.kZero, new Translation3d(0, -1, 0), 1); assertEquals(new Rotation2d(-Math.PI / 2).getRadians(), state4.yaw().getRadians(), epsilon); } @@ -106,145 +103,33 @@ class ShooterPhysicsTest { @Test public void pitchTest() { // check random values are within range - TurretState state1 = ShooterPhysics.getShotParams(Translation2d.kZero, + ShooterPhysics.TurretState state1 = ShooterPhysics.getShotParams(Translation2d.kZero, new Translation3d(1, 0, 0), 1); assertTrue(state1.pitch() >= 0 && state1.pitch() <= Math.PI / 2, state1.toString()); - TurretState state2 = ShooterPhysics.getShotParams(new Translation2d(12.2, -1.3), + ShooterPhysics.TurretState state2 = ShooterPhysics.getShotParams(new Translation2d(12.2, -1.3), new Translation3d(1.1, 12, 11.1).minus(new Translation3d(.2, 1.2, 0)), 22.1); assertTrue(state2.pitch() >= 0 && state2.pitch() <= Math.PI / 2, state2.toString()); - TurretState state3 = ShooterPhysics.getShotParams(new Translation2d(1.9, 9.1), + ShooterPhysics.TurretState state3 = ShooterPhysics.getShotParams(new Translation2d(1.9, 9.1), new Translation3d(11.2, -13.1, 4.1).minus(new Translation3d(-.3, -8.4, 0)), 5.6); assertTrue(state3.pitch() >= 0 && state3.pitch() <= Math.PI / 2, state3.toString()); // try a steep shot - TurretState state4 = ShooterPhysics.getShotParams(Translation2d.kZero, + ShooterPhysics.TurretState state4 = ShooterPhysics.getShotParams(Translation2d.kZero, new Translation3d(1, 0, 99), 100); assertTrue(state4.pitch() >= Math.PI * 7 / 16 && state4.pitch() <= Math.PI / 2, state4.toString()); - TurretState state5 = ShooterPhysics.getShotParams(Translation2d.kZero, + ShooterPhysics.TurretState state5 = ShooterPhysics.getShotParams(Translation2d.kZero, new Translation3d(1, 0, 0), 100); assertTrue(state5.pitch() >= Math.PI * 7 / 16 && state5.pitch() <= Math.PI / 2, state5.toString()); // try a shallow shot - TurretState state6 = ShooterPhysics.getShotParams(Translation2d.kZero, + ShooterPhysics.TurretState state6 = ShooterPhysics.getShotParams(Translation2d.kZero, new Translation3d(100, 50, 1), 2); assertTrue(state6.pitch() >= 0 && state6.pitch() <= Math.PI / 16, state6.toString()); } - @Test - public void velocityTest() { - - // var t1 = new Translation3d(100, 0, 0); - // for (int i = 1; i < 1000; i++) { - // var x = ShooterPhysics.getShotParams(Translation2d.kZero, t1, i / 10.); - // System.out.println(i / 10. + ", " + x.exitVel()); - // } - - var t1 = new Translation3d(100, 0, 0); - var state1 = ShooterPhysics.withMinimumSpeed(Translation2d.kZero, t1); - // check moving either way is higher velocity - var state1Plus = ShooterPhysics.getShotParams(Translation2d.kZero, t1, - state1.height() + 0.2); - var state1Minus = ShooterPhysics.getShotParams(Translation2d.kZero, t1, - state1.height() - 0.2); - assertTrue(state1.exitVel() < state1Plus.exitVel(), state1Plus.toString()); - assertTrue(state1.exitVel() < state1Minus.exitVel(), state1Minus.toString()); - assertEquals(Math.PI / 4, state1.pitch(), epsilon); - - var t2 = new Translation3d(1, 1, 100); - var state2 = ShooterPhysics.withMinimumSpeed(Translation2d.kZero, t2); - // this should get to the minimum height - var state2Plus = ShooterPhysics.getShotParams(Translation2d.kZero, t2, - state2.height() + 0.1); - assertTrue(state2.exitVel() < state2Plus.exitVel(), state2Plus.toString()); - assertEquals(t2.getZ(), state2.height(), epsilon); - - // test with an initial velocity - var t3 = new Translation3d(100, 0, 0); - var v3 = new Translation2d(10, -20); - var state3 = ShooterPhysics.withMinimumSpeed(v3, t3); - // check moving either way is higher velocity - var state3Plus = ShooterPhysics.getShotParams(v3, t3, state3.height() + 0.2); - var state3Minus = ShooterPhysics.getShotParams(v3, t3, state3.height() - 0.2); - assertTrue(state3.exitVel() < state3Plus.exitVel(), state3Plus.toString()); - assertTrue(state3.exitVel() < state3Minus.exitVel(), state3Minus.toString()); - } - - @Test - public void angleTest() { - // for (int i = 21; i < 1000; i++) { - // var x = ShooterPhysics.getShotParams(new Translation2d(-3.81, -3.52), new - // Translation3d(-8.43, -5.58, 2.09), - // i / 10.); - // System.out.println(i / 10. + ", " + x.pitch() + ", " + x.exitVel()); - // } - - var t1 = new Translation3d(10, 0, 0); - var state1 = ShooterPhysics.withAngle(Translation2d.kZero, t1, - Units.degreesToRadians(30)); - assertTrue(state1.isPresent()); - assertEquals(state1.get().pitch(), Units.degreesToRadians(30), epsilon); - // get this as a velocity vector - Translation3d v1 = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero, t1, - state1.get().height()); - assertEquals(ShooterPhysics.cvtShot(v1, state1.get().height()), - state1.get()); - checkTrajectory(Translation3d.kZero, v1, t1, state1.get().height()); - - var t2 = new Translation3d(1, -1, 10); - var state2 = ShooterPhysics.withAngle(Translation2d.kZero, t2, - Units.degreesToRadians(60)); - assertTrue(state2.isEmpty()); - - var t3 = new Translation3d(1.34, 4.2, 2.3); - var iv3 = new Translation2d(12.1, -2.1); - var state3 = ShooterPhysics.withAngle(iv3, t3, - Units.degreesToRadians(45)); - assertTrue(state3.isPresent()); - assertEquals(state3.get().pitch(), Units.degreesToRadians(45), epsilon); - // get this as a velocity vector - Translation3d v3 = ShooterPhysics.getRequiredExitVelocity(iv3, t3, - state3.get().height()); - assertEquals(ShooterPhysics.cvtShot(v3, state3.get().height()), - state3.get()); - checkTrajectory(Translation3d.kZero, v3.plus(new Translation3d(iv3)), t3, - state3.get().height()); - - // just check this converges - var state4 = ShooterPhysics.withAngle(Translation2d.kZero, new Translation3d(10, 10, 3), Math.PI / 2 - 0.01); - assertTrue(state4.isPresent()); - assertEquals(state4.get().pitch(), Math.PI / 2 - 0.01, epsilon); - var state5 = ShooterPhysics.withAngle(Translation2d.kZero, new Translation3d(10, -10, 0), 0.01); - assertTrue(state5.isPresent()); - assertEquals(state5.get().pitch(), 0.01, epsilon); - } - - @Test - public void simpleConstraintsTest() { - // a test where the optimal shot is just plain height - Constraints constraints1 = new Constraints(3, 20, .1, Math.PI / 2 - .1); - var val1 = ShooterPhysics.getConstrainedParams(Translation2d.kZero, new Translation3d(1, 2, 3), constraints1); - assertTrue(val1.isPresent()); - var direct1 = ShooterPhysics.getShotParams(Translation2d.kZero, new Translation3d(1, 2, 3), - constraints1.height()); - assertEquals(direct1.pitch(), val1.get().pitch(), epsilon); - assertEquals(direct1.yaw().getRadians(), val1.get().yaw().getRadians(), epsilon); - assertEquals(direct1.exitVel(), val1.get().exitVel(), epsilon); - - // optimal is the minimum angle - Constraints constraints2 = new Constraints(3, 30, Units.degreesToRadians(45), Math.PI / 2 - .1); - var val2 = ShooterPhysics.getConstrainedParams(Translation2d.kZero, new Translation3d(10, 10, 3), constraints2); - assertTrue(val2.isPresent()); - assertEquals(45, Units.radiansToDegrees(val2.get().pitch()), .1); - - // something impossible - Constraints constraints3 = new Constraints(3, 3, Units.degreesToRadians(45), Math.PI / 2 - .1); - var val3 = ShooterPhysics.getConstrainedParams(Translation2d.kZero, new Translation3d(10, 10, 3), constraints3); - assertTrue(val3.isEmpty(), val3.toString()); - } - // test using a simple physics simulation @Test public void simulatedTest() { @@ -256,12 +141,12 @@ class ShooterPhysicsTest { // compute 1000 random shots and simulate them for (int i = 0; i < 1000; i++) { - Translation2d initPos = new Translation2d(rng.nextDouble(-10, 10), rng.nextDouble(-10, 10)); - Translation3d target = new Translation3d(rng.nextDouble(-10, 10), rng.nextDouble(-10, 10), - rng.nextDouble(1, 11)); - Translation2d initVel = new Translation2d(rng.nextDouble(-5, 5), rng.nextDouble(-5, 5)); + Translation2d initPos = new Translation2d(rng.nextDouble() * 20 - 10, rng.nextDouble() * 20 - 10); + Translation3d target = new Translation3d(rng.nextDouble() * 20 - 10, rng.nextDouble() * 20 - 10, + rng.nextDouble() * 10 + 1); + Translation2d initVel = new Translation2d(rng.nextDouble() * 10 - 5, rng.nextDouble() * 10 - 5); Translation3d robotToTarget = target.minus(new Translation3d(initPos)); - double arcHeight = target.getZ() + rng.nextDouble(10); + double arcHeight = target.getZ() + rng.nextDouble() * 10; Translation3d exitVel = ShooterPhysics.getRequiredExitVelocity(initVel, robotToTarget, arcHeight); @@ -270,37 +155,6 @@ class ShooterPhysicsTest { } } - @Test - public void simulatedConstraintsTest() { - Random rng = new Random(6328); - - for (int i = 0; i < 10_000; i++) { - Translation2d initPos = new Translation2d(rng.nextDouble(-10, 10), rng.nextDouble(-10, 10)); - Translation3d target = new Translation3d(rng.nextDouble(-10, 10), rng.nextDouble(-10, 10), - rng.nextDouble(1, 11)); - Translation2d initVel = new Translation2d(rng.nextDouble(-5, 5), rng.nextDouble(-5, 5)); - Translation3d robotToTarget = target.minus(new Translation3d(initPos)); - - // can't have a min angle lower than 75° because that breaks things - double minAngle = rng.nextDouble(.01, Math.PI / 2 - Units.degreesToRadians(15)); - Constraints constraints = new Constraints(rng.nextDouble(20) + .001, rng.nextDouble(30) + .001, minAngle, - rng.nextDouble(minAngle + .01, Math.PI / 2 - .01)); - - var state = ShooterPhysics.getConstrainedParams(initVel, robotToTarget, constraints); - - if (state.isPresent()) { - assertTrue(state.get().satisfies(constraints)); - - // check going down breaks a constraint; we've found the minimum - var lower = ShooterPhysics.withAngle(initVel, robotToTarget, state.get().pitch() - .1); - assertTrue( - lower.isEmpty() || !lower.get().satisfies(constraints) - || lower.get().height() > state.get().height(), - String.format("%s was better than %s", lower.toString(), state.toString())); - } - } - } - private class PhysicsObject { private Translation3d pos; private Translation3d vel; @@ -330,7 +184,7 @@ class ShooterPhysicsTest { + " and peak height " + requiredHeight + "."); messages.add("position, velocity"); // column headers - while (object.pos.getZ() > -epsilon || firstLoop) { + while (object.pos.getZ() > 0 || firstLoop) { messages.add("" + object.pos + ", " + object.vel); if (object.pos.getZ() + tolerance >= requiredHeight) -- 2.39.5