diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/GreedyBallisticCalculator.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/GreedyBallisticCalculator.java index f15fb8b..95fc046 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/GreedyBallisticCalculator.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/GreedyBallisticCalculator.java @@ -22,9 +22,7 @@ public class GreedyBallisticCalculator extends BallisticCalculator { Angle robotAngle, Velocity3d shooterVelocity ) { - if(initParam == null) { - initParam = model.getInitParam(); - } + model.paramComputedReset(); LinkedList
candidate_params = new LinkedList<>(); candidate_params.addFirst(initParam); @@ -32,8 +30,9 @@ public class GreedyBallisticCalculator extends BallisticCalculator { while(!candidate_params.isEmpty()) { // Evaluate the candidates P param = candidate_params.removeFirst(); - Velocity3d candidate = model.getCandidateVelocity(param); + if(model.paramComputed(param)) continue; + Velocity3d candidate = model.getCandidateVelocity(param); if(candidate != null) { BCEvalParams evalParams = new BCEvalParams(robotVelocity, robotAngle, shooterVelocity, candidate); Double penalty = this.evaluateCandidate(model, evalParams);