From: mixxlto Date: Thu, 15 Jan 2026 05:23:48 +0000 (-0800) Subject: renamed sloppy variables X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=7b38365fa4ffde71f7a1abf400a4d33342668581;p=FRC2026.git renamed sloppy variables --- diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java index cb8640f..8d0ce40 100644 --- a/src/main/java/frc/robot/util/ShooterPhysics.java +++ b/src/main/java/frc/robot/util/ShooterPhysics.java @@ -9,23 +9,23 @@ import frc.robot.constants.Constants; class ShooterPhysics { // pitch in radians, going up from the horizontal - // speed in m/s - public record TurretState(Rotation2d yaw, double pitch, double speed) { + // exit velocity speed in m/s + public record TurretState(Rotation2d yaw, double pitch, double exit_vel) { }; @CheckReturnValue - public static TurretState getShotParams(Translation2d initialVelocity, Translation2d robot, Translation3d target, + public static TurretState getShotParams(Translation2d robotVelocity, Translation2d robot, Translation3d target, double height) { Translation3d robotToTarget = target.minus(new Translation3d(robot)); - Translation3d impulse = getRequiredImpulse(initialVelocity, robotToTarget, height); + Translation3d exit_vel = getRequiredExitVelocity(robotVelocity, robotToTarget, height); - Translation2d as2d = impulse.toTranslation2d(); + Translation2d as2d = exit_vel.toTranslation2d(); Rotation2d yaw = as2d.getAngle(); double magnitude2d = as2d.getDistance(Translation2d.kZero); - double pitch = new Translation2d(magnitude2d, impulse.getZ()).getAngle().getRadians(); + double pitch = new Translation2d(magnitude2d, exit_vel.getZ()).getAngle().getRadians(); pitch %= Math.PI * 2; - double speed = impulse.getDistance(Translation3d.kZero); + double speed = exit_vel.getDistance(Translation3d.kZero); return new TurretState(yaw, pitch, speed); } @@ -33,40 +33,40 @@ class ShooterPhysics { // assumes shot from (0, 0, 0) // only public for unit testing, don't actually use this directly @CheckReturnValue - public static Translation3d getRequiredImpulse(Translation2d initialVelocity, Translation3d target, double peakZ) { - // z = v_z_impulse * t - .5 * g * t² + public static Translation3d getRequiredExitVelocity(Translation2d robotVelocity, Translation3d target, double peakZ) { + // z = v_z_exit_vel * t - .5 * g * t² // want vertex of this equation to equal peakZ - // t_vertex = -v_z_impulse / -g - // t_vertex = v_z_impulse / g - // z_vertex = v_z_impulse * (v_z_impulse / g) - .5 * g * (v_z_impulse / g)² - // peakZ = v_z_impulse² / g - .5 * v_z_impulse² / g - // peakZ = .5 * v_z_impulse² / g - // v_z_impulse² = 2 * peakZ * g - // v_z_impulse = √(2 * peakZ * g) - double zImpulse = Math.sqrt(2 * peakZ * Constants.GRAVITY_ACCELERATION); + // t_vertex = -v_z_exit_vel / -g + // t_vertex = v_z_exit_vel / g + // z_vertex = v_z_exit_vel * (v_z_exit_vel / g) - .5 * g * (v_z_exit_vel / g)² + // peakZ = v_z_exit_vel² / g - .5 * v_z_exit_vel² / g + // peakZ = .5 * v_z_exit_vel² / g + // v_z_exit_vel² = 2 * peakZ * g + // v_z_exit_vel = √(2 * peakZ * g) + double zExit_vel = Math.sqrt(2 * peakZ * Constants.GRAVITY_ACCELERATION); // now we need time to hit target - // z_target = v_z_impulse * t - .5 * g * t² - // 0 = -.5 * g * t² + v_z_impulse * t - z_target + // z_target = v_z_exit_vel * t - .5 * g * t² + // 0 = -.5 * g * t² + v_z_exit_vel * t - z_target // quadratic formula - // t = (-v_z_impulse ± √(4 * (-.5 * g) * (-z_target))) / (2 * -.5 * g) - // t = (-v_z_impulse ± √(2 * g * z_target)) / -g + // t = (-v_z_exit_vel ± √(4 * (-.5 * g) * (-z_target))) / (2 * -.5 * g) + // t = (-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 = (-zImpulse - Math.sqrt(2 * Constants.GRAVITY_ACCELERATION * target.getZ())) + double t = (-zExit_vel - Math.sqrt(2 * Constants.GRAVITY_ACCELERATION * target.getZ())) / -Constants.GRAVITY_ACCELERATION; if (t < 0) throw new RuntimeException("Time should never be negative (got t=" + t + ")."); - // calculate x and z impulse - // x = (v_x_robot + v_x_impulse) * t - // x_target = (v_x_robot + v_x_impulse) * t_target - // v_x_robot + v_x_impulse = x_target / t_target - // v_x_impulse = x_target / t_target - v_x_robot - double xImpulse = target.getX() / t - initialVelocity.getX(); + // calculate x and z exit_vel + // x = (v_x_robot + v_x_exit_vel) * t + // x_target = (v_x_robot + v_x_exit_vel) * t_target + // v_x_robot + v_x_exit_vel = x_target / t_target + // v_x_exit_vel = x_target / t_target - v_x_robot + double xExit_vel = target.getX() / t - robotVelocity.getX(); // same for y - double yImpulse = target.getY() / t - initialVelocity.getY(); - return new Translation3d(xImpulse, yImpulse, zImpulse); + double yExit_vel = target.getY() / t - robotVelocity.getY(); + return new Translation3d(xExit_vel, yExit_vel, zExit_vel); } } diff --git a/src/test/java/frc/robot/util/ShooterPhysicsTest.java b/src/test/java/frc/robot/util/ShooterPhysicsTest.java index fd6631b..09b8058 100644 --- a/src/test/java/frc/robot/util/ShooterPhysicsTest.java +++ b/src/test/java/frc/robot/util/ShooterPhysicsTest.java @@ -25,32 +25,32 @@ class ShooterPhysicsTest { @Test public void basicImpulseTest() { - Translation3d transform = ShooterPhysics.getRequiredImpulse(Translation2d.kZero, Translation3d.kZero, 1); + Translation3d transform = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero, Translation3d.kZero, 1); assertEquals(0, transform.getX(), epsilon); assertEquals(0, transform.getY(), epsilon); assertEquals(4.427, transform.getZ(), epsilon); - Translation3d transform2 = ShooterPhysics.getRequiredImpulse(Translation2d.kZero, new Translation3d(10, 0, 0), + Translation3d transform2 = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero, new Translation3d(10, 0, 0), 1); assertTrue(transform2.getX() > 0, transform2.toString()); assertEquals(0, transform2.getY(), epsilon); assertEquals(4.427, transform2.getZ(), epsilon); - Translation3d transform3 = ShooterPhysics.getRequiredImpulse(Translation2d.kZero, + Translation3d transform3 = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero, new Translation3d(0, -6, 0), 1); assertEquals(0, transform3.getX(), epsilon); assertTrue(transform3.getY() < 0, transform3.toString()); assertEquals(4.427, transform3.getZ(), epsilon); - Translation3d transform4 = ShooterPhysics.getRequiredImpulse(Translation2d.kZero, + Translation3d transform4 = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero, new Translation3d(6, 3, 0), 2); assertEquals(6. / 3, transform4.getX() / transform4.getY(), epsilon); - Translation3d transform5 = ShooterPhysics.getRequiredImpulse(Translation2d.kZero, + Translation3d transform5 = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero, new Translation3d(12.14, 3.21, 0), 2); assertEquals(12.14 / 3.21, transform5.getX() / transform5.getY(), epsilon); - Translation3d transform6 = ShooterPhysics.getRequiredImpulse(Translation2d.kZero, + Translation3d transform6 = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero, new Translation3d(-16.32, 3.3, 0), 2); assertEquals(-16.32 / 3.3, transform6.getX() / transform6.getY(), epsilon); } @@ -58,8 +58,8 @@ class ShooterPhysicsTest { @Test public void initialVelocityTest() { Translation2d initialVelocityDiff = new Translation2d(5.1, -6.5); - Translation3d transform = ShooterPhysics.getRequiredImpulse(Translation2d.kZero, new Translation3d(1, 2, 3), 4); - Translation3d transform2 = ShooterPhysics.getRequiredImpulse(initialVelocityDiff, new Translation3d(1, 2, 3), + Translation3d transform = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero, new Translation3d(1, 2, 3), 4); + Translation3d transform2 = ShooterPhysics.getRequiredExitVelocity(initialVelocityDiff, new Translation3d(1, 2, 3), 4); assertEquals(transform.getX(), transform2.getX() + initialVelocityDiff.getX(), epsilon); assertEquals(transform.getY(), transform2.getY() + initialVelocityDiff.getY(), epsilon);