public static Optional<TurretState> getConstrainedParams(Translation2d robotVelocity, Translation3d robotToTarget,
Constraints constraints) {
// establish a lower bound
- double minHeight = Math.max(robotToTarget.getZ(), constraints.height());
+ double minHeight = Math.max(Math.max(robotToTarget.getZ(), constraints.height()), 0.01);
Optional<TurretState> withMinPitch = withAngle(robotVelocity, robotToTarget, constraints.minPitch());
- if (withMinPitch.isPresent()) {
- minHeight = Math.min(minHeight, withMinPitch.get().height());
- }
+ if (withMinPitch.isPresent())
+ minHeight = Math.max(minHeight, withMinPitch.get().height());
+
TurretState withMinHeight = cvtShot(getRequiredExitVelocity(robotVelocity, robotToTarget, minHeight),
minHeight);
if (withMinHeight.satisfies(constraints))
// this is not equivalent to t <= 0 because of NaNs
if (!(t > 0))
- throw new RuntimeException("Time should never be negative (got t=" + t + ").");
+ throw new RuntimeException("Time should never be negative (got t=" + t + " with target: " + target
+ + " and peakZ: " + peakZ + ").");
// calculate x and z exit_vel
// x = (v_x_robot + v_x_exit_vel) * t
public static Optional<TurretState> withAngle(Translation2d initialVelocity, Translation3d target,
double pitch, double tolerance) {
- // guess a peak height
- double guess = target.getZ() + 2;
+ 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 = cvtShot(getRequiredExitVelocity(initialVelocity, target, effectiveMinHeight),
+ effectiveMinHeight);
+ // if a shot requires going up a kilometer, it's probably not doable
+ TurretState second = cvtShot(getRequiredExitVelocity(initialVelocity, target, 1000.), 1000.);
+
+ if (first.pitch() > pitch + tolerance)
+ return Optional.empty();
+ else if (second.pitch() < pitch)
+ return Optional.of(second); // it's close enough
+
int maxIters = 50;
+ var range = new Pair<TurretState, TurretState>(first, second);
while (maxIters >= 0) {
maxIters--;
+ assert range.getSecond().height() > range.getFirst().height();
- // 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);
- TurretState polar = cvtShot(guessVelocity, guess);
- double difference = pitch - polar.pitch();
- // System.out.println(guess + "\t\t" + guessVelocity + "\t\t" + polar.pitch() +
- // "\t\t" + difference);
+ double guessHeight = (range.getFirst().height() + range.getSecond().height()) / 2;
+ TurretState guess = cvtShot(getRequiredExitVelocity(initialVelocity, target, guessHeight), guessHeight);
- // we've already hit minimum height and are trying to go lower
- if (guess <= target.getZ() && difference < 0)
- return Optional.empty();
+ // System.out.println(range + "\t\tguess=" + guess);
- if (Math.abs(difference) <= tolerance)
- return Optional.of(polar);
+ if (range.getSecond().pitch() - range.getFirst().pitch() <= tolerance) {
+ // we've found a valid angle
+ if (Math.abs(guess.pitch() - pitch) <= tolerance)
+ return Optional.of(guess);
+ // we've narrowed the range but haven't found a valid angle
+ // should be covered by the checks before the loop
+ assert false;
+ }
- guess += difference * 10;
+ if (guess.pitch() > pitch)
+ range = new Pair<TurretState, TurretState>(range.getFirst(), guess);
+ else
+ range = new Pair<TurretState, TurretState>(guess, range.getSecond());
}
- throw new RuntimeException("Solving for angle did not converge.");
+ throw new RuntimeException("Solving for angle did not converge (velocity: " + initialVelocity + ", target: "
+ + target + ", pitch: " + pitch + ", tolerance: " + tolerance + ").");
}
}
@Test
public void angleTest() {
- // var t1 = new Translation3d(1, 2, 3);
+ // var t1 = new Translation3d(10, 10, 3);
// for (int i = 31; i < 1000; i++) {
// var x = ShooterPhysics.getShotParams(Translation2d.kZero, t1, i / 10.);
// System.out.println(i / 10. + ", " + x.pitch());
// }
- var t1 = new Translation3d(100, 0, 0);
+ var t1 = new Translation3d(10, 0, 0);
var state1 = ShooterPhysics.withAngle(Translation2d.kZero, t1,
Units.degreesToRadians(30));
assertTrue(state1.isPresent());
state1.get());
checkTrajectory(Translation3d.kZero, v1, t1, state1.get().height());
- var t2 = new Translation3d(1, -1, 100);
+ var t2 = new Translation3d(1, -1, 10);
var state2 = ShooterPhysics.withAngle(Translation2d.kZero, t2,
Units.degreesToRadians(60));
assertTrue(state2.isEmpty());
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 - .1);
+ 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),
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);
+ // 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(Units.degreesToRadians(45), val2.get().pitch(), epsilon);
+ assertEquals(45, Units.radiansToDegrees(val2.get().pitch()), .1);
}
// test using a simple physics simulation