package frc.robot.util;
-import java.lang.Math;
+import com.google.errorprone.annotations.CheckReturnValue;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Rotation2d;
-
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
import frc.robot.constants.Constants;
class ShooterPhysics {
// speed in m/s
public record TurretState(Rotation2d yaw, double pitch, double speed){};
- public TurretState getShotParams(Translation2d initialVelocity, Translation2d robot, Translation3d target, double height) {
+ @CheckReturnValue
+ public static TurretState getShotParams(Translation2d initialVelocity, Translation2d robot, Translation3d target, double height) {
Translation3d robotToTarget = target.minus(new Translation3d(robot));
Translation3d impulse = getRequiredImpulse(initialVelocity, robotToTarget, height);
}
// assumes shot from (0, 0, 0)
- private static Translation3d getRequiredImpulse(Translation2d initialVelocity, Translation3d target, double peakZ) {
+ // 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²
// want vertex of this equation to equal peakZ
// t_vertex = -v_z_impulse / -g
// z_target = v_z_impulse * t - .5 * g * t²
// 0 = -.5 * g * t² + v_z_impulse * 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_impulse ± √(4 * (-.5 * g) * (-z_target))) / (2 * -.5 * g)
+ // t = (-v_z_impulse ± √(2 * g * z_target)) / -g
// onlz use + because we only want the part where it's coming down
- double t = (zImpulse + Math.sqrt(2 * Constants.GRAVITY_ACCELERATION * target.getZ()))
+ double t = (-zImpulse + Math.sqrt(2 * Constants.GRAVITY_ACCELERATION * target.getZ()))
/ -Constants.GRAVITY_ACCELERATION;
+ if (t < 0)
+ throw new RuntimeException("Time should never be negative (got: " + t + ").");
+
// calculate x and z impulse
// x = (v_x_robot + v_x_impulse) * t
// x_target = (v_x_robot + v_x_impulse) * t_target
--- /dev/null
+
+package frc.robot.util;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+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;
+
+class ShooterPhysicsTest {
+ private static final double epsilon = .001;
+
+ @BeforeEach
+ public void prepare() {
+ }
+
+ @AfterEach
+ public void cleanup() {
+ }
+
+ @Test
+ public void basicImpulseTest() {
+ Translation3d transform = ShooterPhysics.getRequiredImpulse(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),
+ 1);
+ assertTrue(transform2.getX() > 0, transform2.toString());
+ assertEquals(0, transform2.getY(), epsilon);
+ assertEquals(4.427, transform2.getZ(), epsilon);
+
+ Translation3d transform3 = ShooterPhysics.getRequiredImpulse(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,
+ new Translation3d(6, 3, 0), 2);
+ assertEquals(6. / 3, transform4.getX() / transform4.getY(), epsilon);
+
+ Translation3d transform5 = ShooterPhysics.getRequiredImpulse(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,
+ new Translation3d(-16.32, 3.3, 0), 2);
+ assertEquals(-16.32 / 3.3, transform6.getX() / transform6.getY(), epsilon);
+ }
+
+ @Test
+ public void angleTest() {
+ ShooterPhysics.TurretState state1 = ShooterPhysics.getShotParams(Translation2d.kZero, 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());
+
+ ShooterPhysics.TurretState state2 = ShooterPhysics.getShotParams(Translation2d.kZero, Translation2d.kZero,
+ new Translation3d(0, 1, 0), 1);
+ assertEquals(new Rotation2d(Math.PI / 2).getRadians(), state2.yaw().getRadians(), epsilon);
+
+ ShooterPhysics.TurretState state3 = ShooterPhysics.getShotParams(Translation2d.kZero, Translation2d.kZero,
+ new Translation3d(-1, 0, 0), 1);
+ assertEquals(new Rotation2d(Math.PI).getRadians(), state3.yaw().getRadians(), epsilon);
+
+ ShooterPhysics.TurretState state4 = ShooterPhysics.getShotParams(Translation2d.kZero, Translation2d.kZero,
+ new Translation3d(0, -1, 0), 1);
+ assertEquals(new Rotation2d(-Math.PI / 2).getRadians(), state4.yaw().getRadians(), epsilon);
+ }
+
+}