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;
+ if (guess.height() < lastValid.height())
+ lastValid = guess;
newRange = new Pair<TurretState, TurretState>(guess, newRange.getSecond());
} else {
newRange = new Pair<TurretState, TurretState>(newRange.getFirst(), guess);
// peakZ = .5 * v_z_exit_vel² / g
// v_z_exit_vel² = 2 * peakZ * g
// v_z_exit_vel = √(2 * peakZ * g)
- double zExitVel = Math.sqrt(2 * peakZ * Constants.GRAVITY_ACCELERATION);
+ // 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);
// now we need time to hit target
// z_target = v_z_exit_vel * t - .5 * g * t²
// 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(Math.pow(zExitVel, 2) - 2 * Constants.GRAVITY_ACCELERATION * target.getZ()))
+ double t = (-zExitVel - Math.sqrt(zExitVelSquared - 2 * Constants.GRAVITY_ACCELERATION * target.getZ()))
/ -Constants.GRAVITY_ACCELERATION;
- if (t < 0)
+ // this is not equivalent to t <= 0 because of NaNs
+ if (!(t > 0))
throw new RuntimeException("Time should never be negative (got t=" + t + ").");
// calculate x and z exit_vel
Translation3d guessVelocity = getRequiredExitVelocity(initialVelocity, target, guess);
Translation3d guessVelocityMore = getRequiredExitVelocity(initialVelocity, target, guess + 0.1);
double derivative = (guessVelocityMore.getNorm() - guessVelocity.getNorm()) / 0.1;
- // System.out.println(guess + "\t\t" + guessVelocity.getNorm() + "\t\t" + derivative);
+ // System.out.println(guess + "\t\t" + guessVelocity.getNorm() + "\t\t" +
+ // derivative);
// we've already hit minimum height and are trying to go lower
if (guess <= effectiveMinHeight && derivative > 0)
public static Optional<TurretState> withAngle(Translation2d initialVelocity, Translation3d target,
double pitch, double tolerance) {
-
// guess a peak height
double guess = target.getZ() + 2;
int maxIters = 50;
Translation3d guessVelocity = getRequiredExitVelocity(initialVelocity, target, guess);
TurretState polar = cvtShot(guessVelocity, guess);
double difference = pitch - polar.pitch();
- // System.out.println(guess + "\t\t" + difference);
+ // System.out.println(guess + "\t\t" + guessVelocity + "\t\t" + polar.pitch() +
+ // "\t\t" + difference);
// we've already hit minimum height and are trying to go lower
if (guess <= target.getZ() && difference < 0)
import org.junit.jupiter.api.AfterEach;
import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Disabled;
import org.junit.jupiter.api.Test;
import edu.wpi.first.math.geometry.Rotation2d;
state1.height() - 0.1);
assertTrue(state1.exitVel() < state1Plus.exitVel(), state1Plus.toString());
assertTrue(state1.exitVel() < state1Minus.exitVel(), state1Minus.toString());
- assertEquals(Math.PI/4, state1.pitch(), epsilon);
+ assertEquals(Math.PI / 4, state1.pitch(), epsilon);
var t2 = new Translation3d(1, 1, 100);
var state2 = ShooterPhysics.withMinimumSpeed(Translation2d.kZero, t2);
@Test
public void angleTest() {
- // var t1 = new Translation3d(100, 0, 0);
- // for (int i = 0; i < 1000; i++) {
+ // var t1 = new Translation3d(1, 2, 3);
+ // for (int i = 31; i < 1000; i++) {
// var x = ShooterPhysics.getShotParams(Translation2d.kZero, t1, i / 10.);
// System.out.println(i / 10. + ", " + x.pitch());
// }
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());
+ 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, 100);
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());
+ 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());
}
- @Disabled
@Test
public void simpleConstraintsTest() {
- Constraints constraints = new Constraints(3, 20, .1, Math.PI - .1);
- var val1 = ShooterPhysics.getConstrainedParams(Translation2d.kZero, new Translation3d(1, 2, 3), constraints);
+ // a test where the optimal shot is just plain height
+ Constraints constraints1 = new Constraints(3, 20, .1, Math.PI - .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),
- constraints.height());
- assertEquals(direct1, val1);
+ 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);
+
+ Constraints constraints2 = new Constraints(3, 20, Units.degreesToRadians(45), Math.PI - .2);
+ var val2 = ShooterPhysics.getConstrainedParams(Translation2d.kZero, new Translation3d(2, 2, 3), constraints2);
+ assertTrue(val2.isPresent());
+ assertEquals(Units.degreesToRadians(45), val2.get().pitch(), epsilon);
}
// test using a simple physics simulation