diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/BallisticModel.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/BallisticModel.java
index 0e91fca..224af83 100644
--- a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/BallisticModel.java
+++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/BallisticModel.java
@@ -14,19 +14,27 @@ public abstract class BallisticModel {
this.posTarget = posTarget;
}
+ //
+ // Base getters
+ //
+
+ public Translation3d getPosLaunch() {
+ return this.posLaunch;
+ }
+
+ public Translation3d getPosTarget() {
+ return this.posTarget;
+ }
+
+ //
+ // Parameter Manipulation
+ //
+
/**
* @return Returns a recommended starting parameter
*/
public abstract P getInitParam();
- /**
- * Get an abstract velocity (in the same coordinate space as posLaunch and
- * posTarget) which will pass thru
- *
- * @param param The parameters used to determine this candidate
- * @return The computed result for the given parameter
- */
- public abstract Velocity3d getCandidateVelocity(P param);
/**
* Get parameters that are "adjacent" to the input paramter (i.e. generate
@@ -37,8 +45,25 @@ public abstract class BallisticModel
{
*/
public abstract P[] getNeighborParams(P param);
+
+ //
+ // Velocity modelling
+ //
+
/**
- * Get the position of the projectile at a given time offset
+ * Get an initial velocity (in the same coordinate space as posLaunch and
+ * posTarget) which will pass thru posLaunch and posTarget with the given
+ * model configurations and parameters
+ *
+ * @param param The parameters used to determine this candidate
+ * @return The computed result for the given parameter
+ */
+ public abstract Velocity3d getCandidateVelocity(P param);
+
+
+ /**
+ * Get the position of the projectile at a given time offset for a given
+ * starting velocity
*
* @param velocity The velocity to check
* @param time The time to check
@@ -46,6 +71,10 @@ public abstract class BallisticModel
{
*/
public abstract Translation3d positionAtTime(Velocity3d velocity, Time time);
+ //
+ // Computed parameter check
+ //
+
/**
* Check if a given parameter has had a computed velocity
* @param param The param to check
diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ParabolicTimeOfFlightBM.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ParabolicTimeOfFlightBM.java
index 5547324..ba4f365 100644
--- a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ParabolicTimeOfFlightBM.java
+++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ParabolicTimeOfFlightBM.java
@@ -14,8 +14,7 @@ import edu.wpi.first.units.measure.Time;
public class ParabolicTimeOfFlightBM extends ReversableRadialBM {
-
- private final LinearAcceleration k_gravity = Units.MetersPerSecondPerSecond.of(-9.81);
+ public static final LinearAcceleration k_gravity = Units.MetersPerSecondPerSecond.of(-9.81);
protected final Time tofMin;
protected final Time tofMax;
@@ -54,20 +53,41 @@ public class ParabolicTimeOfFlightBM extends ReversableRadialBM {
this.computedSeconds = new HashSet<>();
}
+ //
+ // Base getters
+ //
- public Time getInitParam() {
- return this.tofMin.plus(this.tofMax).div(2);
+ public Time getTOFMin() {
+ return this.tofMin;
+ }
+
+ public Time getTOFMax() {
+ return this.tofMax;
+ }
+
+ public Time getTOFStep() {
+ return this.tofStep;
+ }
+
+ public Translation3d getTargetPosRelative() {
+ return this.targetPosRelative;
+ }
+
+ public Distance getRelativeHorizontalDistance() {
+ return this.relativeHorizontalDistance;
+ }
+
+ public Angle getAzimuth() {
+ return this.azimuth;
}
- public Velocity3d getCandidateVelocity(Time param) {
- this.computedSeconds.add(param.in(Units.Seconds));
- return new Velocity3d(
- this.relativeHorizontalDistance.div(param),
- this.targetPosRelative.getMeasureZ().div(param)
- .plus(k_gravity.times(param).times(0.5)),
- azimuth
- );
+ //
+ // Parameter manipulation
+ //
+
+ public Time getInitParam() {
+ return this.tofMin.plus(this.tofMax).div(2);
}
@@ -84,6 +104,21 @@ public class ParabolicTimeOfFlightBM extends ReversableRadialBM {
}
+ //
+ // Velocity modelling
+ //
+
+ public Velocity3d getCandidateVelocity(Time param) {
+ this.computedSeconds.add(param.in(Units.Seconds));
+ return new Velocity3d(
+ this.relativeHorizontalDistance.div(param),
+ this.targetPosRelative.getMeasureZ().div(param)
+ .minus(k_gravity.times(param).times(0.5)),
+ azimuth
+ );
+ }
+
+
public Translation3d positionAtTime(Velocity3d velocity, Time time) {
return new Translation3d(
this.posLaunch.getMeasureX()
@@ -97,11 +132,19 @@ public class ParabolicTimeOfFlightBM extends ReversableRadialBM {
}
+ //
+ // Reverse functions
+ //
+
public Time getTimeAtRadius(Velocity3d candidate, Distance radialDistance) {
return radialDistance.div(candidate.getHorizontalVelocity());
}
+ //
+ // Computed parameter check
+ //
+
public boolean paramComputed(Time param) {
return this.computedSeconds.contains(param.in(Units.Seconds));
}
diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ReversableRadialBM.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ReversableRadialBM.java
index 032c6a7..34f3905 100644
--- a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ReversableRadialBM.java
+++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ReversableRadialBM.java
@@ -16,6 +16,11 @@ public abstract class ReversableRadialBM extends BallisticModel
{
super(posLaunch, posTarget);
}
+
+ //
+ // Reverse functions
+ //
+
/**
* Get the time taken to travel a given radial distance from posLaunch towards posTarget
*
@@ -36,7 +41,8 @@ public abstract class ReversableRadialBM
extends BallisticModel
{
*/
public Time getTimeAtX(Velocity3d candidate, Distance xDistance) {
Distance radialDistance = Units.Meters.of(
- xDistance.in(Units.Meters)/Math.cos(candidate.getAzimuthAngle().in(Units.Radians))
+ xDistance.minus(posLaunch.getMeasureX()).in(Units.Meters)
+ / Math.cos(candidate.getAzimuthAngle().in(Units.Radians))
);
return this.getTimeAtRadius(candidate, radialDistance);
}
@@ -52,11 +58,13 @@ public abstract class ReversableRadialBM
extends BallisticModel
{
*/
public Time getTimeAtY(Velocity3d candidate, Distance yDistance) {
Distance radialDistance = Units.Meters.of(
- yDistance.in(Units.Meters)/Math.sin(candidate.getAzimuthAngle().in(Units.Radians))
+ yDistance.minus(posLaunch.getMeasureY()).in(Units.Meters)
+ / Math.sin(candidate.getAzimuthAngle().in(Units.Radians))
);
return this.getTimeAtRadius(candidate, radialDistance);
}
+
/**
* Get the time to target on a given candidate trajectory
*
diff --git a/lib/src/test/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ParabolicTimeOfFlightBMTest.java b/lib/src/test/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ParabolicTimeOfFlightBMTest.java
new file mode 100644
index 0000000..d743c56
--- /dev/null
+++ b/lib/src/test/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ParabolicTimeOfFlightBMTest.java
@@ -0,0 +1,139 @@
+package org.fairportrobotics.frc.robolib.trajectory.ballistics.models;
+
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Time;
+
+import static org.junit.jupiter.api.Assertions.*;
+
+import org.fairportrobotics.frc.robolib.trajectory.Velocity3d;
+
+public class ParabolicTimeOfFlightBMTest {
+ private ParabolicTimeOfFlightBM getModel() {
+ return new ParabolicTimeOfFlightBM(
+ new Translation3d(Units.Meters.of(1), Units.Meters.of(1), Units.Meters.of(1)),
+ new Translation3d(Units.Meters.of(4), Units.Meters.of(5), Units.Meters.of(2)),
+ Units.Seconds.of(1.0), Units.Seconds.of(3), Units.Seconds.of(1.0)
+ );
+ }
+
+ @Test void testParamIterator() {
+ ParabolicTimeOfFlightBM model = getModel();
+
+ // Define the initial param
+ Time initParam = model.getInitParam();
+ assertEquals(2.0, initParam.in(Units.Seconds), "Init param");
+ assertArrayEquals(
+ new Time[]{Units.Seconds.of(1.0), Units.Seconds.of(3.0)},
+ model.getNeighborParams(initParam), "Neighbors"
+ );
+ }
+
+ @Test void testSolutionVelocity() {
+ ParabolicTimeOfFlightBM model = getModel();
+ Time tof = Units.Seconds.of(1.0);
+
+ // Compute the solution
+ Velocity3d result = model.getCandidateVelocity(tof);
+ assertEquals(new Velocity3d(
+ Units.MetersPerSecond.of(5),
+ Units.MetersPerSecond.of(1).minus(
+ ParabolicTimeOfFlightBM.k_gravity.times(0.5).times(tof)
+ ),
+ Units.Radians.of(Math.atan2(4, 3))
+ ), result, "Compute solution");
+
+ // Check that positions match expected values
+ assertEquals(
+ model.getPosLaunch(),
+ model.positionAtTime(result, Units.Seconds.of(0)),
+ "Launch position"
+ );
+ assertEquals(
+ model.getPosTarget(),
+ model.positionAtTime(result, tof),
+ "Target position check"
+ );
+
+ Time midTime = Units.Seconds.of(0.5);
+ assertEquals(
+ new Translation3d(
+ Units.Meters.of(2.5), // 1+3*0.5
+ Units.Meters.of(3), // 1+4*0.5
+ Units.Meters.of(1.0)
+ .plus(result.getZVelocity().times(midTime))
+ .plus(
+ ParabolicTimeOfFlightBM.k_gravity
+ .times(0.5).times(midTime).times(midTime)
+ )
+ ),
+ model.positionAtTime(result, midTime),
+ "Mid-flight position check"
+ );
+ }
+
+ @Test void testReverseFunctions() {
+ ParabolicTimeOfFlightBM model = getModel();
+ Time tof = Units.Seconds.of(1.0);
+ Velocity3d solution = model.getCandidateVelocity(tof);
+
+ // Test initial position
+ Time t0 = Units.Seconds.of(0.0);
+ assertEquals(t0, model.getTimeAtRadius(solution, Units.Meters.of(0.0)), "Init Position (Rad)");
+ assertEquals(t0, model.getTimeAtX(solution, model.getPosLaunch().getMeasureX()), "Init Position (X)");
+ assertEquals(t0, model.getTimeAtY(solution, model.getPosLaunch().getMeasureY()), "Init Position (Y)");
+
+ // Test target position
+ assertEquals(tof, model.getTimeAtRadius(solution, model.getRelativeHorizontalDistance()), "Final Position (Rad)");
+ assertEquals(tof, model.getTimeAtX(solution, model.getPosTarget().getMeasureX()), "Final Position (X)");
+ assertEquals(tof, model.getTimeAtY(solution, model.getPosTarget().getMeasureY()), "Final Position (Y)");
+ assertEquals(tof, model.getTimeAtTarget(solution));
+
+ // Test mid-flight position
+ Time tMid = Units.Seconds.of(0.5);
+ assertEquals(tMid, model.getTimeAtRadius(solution, model.getRelativeHorizontalDistance().div(2)));
+ assertEquals(tMid, model.getTimeAtX(solution, Units.Meters.of(2.5)));
+ assertEquals(tMid, model.getTimeAtY(solution, Units.Meters.of(3.0)));
+ }
+
+ @Test void testCandidateTracking() {
+ ParabolicTimeOfFlightBM model = getModel();
+ Time[] candidates = {
+ Units.Seconds.of(1.0),
+ Units.Seconds.of(1.5),
+ Units.Seconds.of(2.0),
+ Units.Seconds.of(2.5),
+ Units.Seconds.of(3.0)
+ };
+
+ for(int cur_candidate = 0; cur_candidate < candidates.length; ++cur_candidate) {
+ // Check that existing candidates have been checked
+ for(int seen_candidate = 0; seen_candidate < cur_candidate; ++seen_candidate) {
+ assertTrue(
+ model.paramComputed(candidates[seen_candidate]),
+ String.format(
+ "Cur Candidate: %d, Seen Candidate: %d",
+ cur_candidate, seen_candidate
+ )
+ );
+ }
+
+ // Check that unseen candidates haven't been checked
+ for(int unseen_candidate = cur_candidate; unseen_candidate < candidates.length; ++unseen_candidate) {
+ assertFalse(
+ model.paramComputed(candidates[unseen_candidate]),
+ String.format(
+ "Cur Candidate: %d, Unseen Candidate: %d",
+ cur_candidate, unseen_candidate
+ )
+ );
+ }
+
+ // Compute for the candidate
+ model.getCandidateVelocity(candidates[cur_candidate]);
+ }
+ }
+}