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);
}
- public static Optional<Translation3d> getImpulseForSpeed(Translation2d initialVelocity, Translation3d target,
+
+ // call with default tolerance
- return getImpulseForSpeed(initialVelocity, target, speed, 0.1);
++ public static Optional<Translation3d> getExitVelocityForSpeed(Translation2d initialVelocity, Translation3d target,
+ double speed) {
- public static Optional<Translation3d> getImpulseForSpeed(Translation2d initialVelocity, Translation3d target,
++ return getExitVelocityForSpeed(initialVelocity, target, speed, 0.1);
+ }
+
- Translation3d guessVelocity = getRequiredImpulse(initialVelocity, target, guess);
++ public static Optional<Translation3d> getExitVelocityForSpeed(Translation2d initialVelocity, Translation3d target,
+ double speed, double tolerance) {
+
+ // TODO: detect when the given velocity is insufficient and exit before maxIters
+
+ // guess a peak height
+ double guess = 10;
+ int maxIters = 20;
+ while (maxIters >= 0) {
+ maxIters--;
++ Translation3d guessVelocity = getRequiredExitVelocity(initialVelocity, target, guess);
+ double guessSpeed = guessVelocity.getNorm();
+ double difference = speed - guessSpeed;
+
+ // we've already hit zero height and are trying to go lower
+ if (guess <= 0 && difference < 0)
+ return Optional.empty();
+
+ if (Math.abs(difference) <= tolerance)
+ return Optional.of(guessVelocity);
+
+ guess += difference * 1.7; // experimentally determined value
+ }
+
+ return Optional.empty();
+ }
}