FairportRobotics
/
RoboLib
mirror of https://github.com/FairportRobotics/RoboLib.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

src/robolib/trajectory/ballistics: BCEvalParams 

updated nomenclature

Also, getCandidateShooterVelocityRelative() uses
Velocity3d.rotate()
This commit is contained in:
Ray Healy 2026-02-10 19:38:30 -05:00
parent 2056de0a1a
commit 7c7b44d3e4
1 changed files with 37 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/BCEvalParams.java
View File

@ -13,34 +13,35 @@ public class BCEvalParams {
// //
// Robot motion params // Robot motion params
private final Velocity3d currentGlobalRobotVelocity; private final Velocity3d robotVelocity;
private final Angle currentGlobalRobotAngle; private final Angle robotAngle;
private final Velocity3d currentGlobalShooterVelocity; private final Velocity3d currentShooterVelocity;
// Candidate params // Candidate params
private final Velocity3d candidateGlobalTotalVelocity; private final Velocity3d candidateVelocity;
// Composite value cache // Composite value cache
private Velocity3d cache_candidateGlobalShooterVelocity; private Velocity3d cache_candidateShooterVelocity;
private Velocity3d cache_candidateRelativeShooterVelocity; private Velocity3d cache_candidateShooterVelocityRelative;
// //
// Setup // Setup
// //
public BCEvalParams( public BCEvalParams(
Velocity3d currentGlobalRobotVelocity, Velocity3d robotVelocity,
Angle currentGlobalRobotAngle, Angle robotAngle,
Velocity3d currentGlobalShooterVelocity, Velocity3d currentShooterVelocity,
Velocity3d candidateGlobalTotalVelocity Velocity3d candidateVelocity
) { ) {
this.currentGlobalRobotVelocity = currentGlobalRobotVelocity; this.robotVelocity = robotVelocity;
this.currentGlobalRobotAngle = currentGlobalRobotAngle; this.robotAngle = robotAngle;
this.currentGlobalShooterVelocity = currentGlobalShooterVelocity; this.currentShooterVelocity = currentShooterVelocity;
this.candidateGlobalTotalVelocity = candidateGlobalTotalVelocity;
this.cache_candidateGlobalShooterVelocity = null; this.candidateVelocity = candidateVelocity;
this.cache_candidateRelativeShooterVelocity = null;
this.cache_candidateShooterVelocity = null;
this.cache_candidateShooterVelocityRelative = null;
} }
@ -51,31 +52,32 @@ public class BCEvalParams {
/** /**
* @return The robot's current velocity (in field coordinates) * @return The robot's current velocity (in field coordinates)
*/ */
public Velocity3d getCurrentGlobalRobotVelocity() { public Velocity3d getRobotVelocity() {
return this.currentGlobalRobotVelocity; return this.robotVelocity;
} }
/** /**
* @return The robot's current heading angle (in field coordinates) * @return The robot's current heading angle (in field coordinates)
*/ */
public Angle getCurrentGlobalRobotAngle() { public Angle getRobotAngle() {
return this.currentGlobalRobotAngle; return this.robotAngle;
} }
/** /**
* @return The shooter's current launch velocity (in field coordinates) * @return The shooter's current launch velocity (in field coordinates)
*/ */
public Velocity3d getCurrentGlobalShooterVelocity() { public Velocity3d getCurrentShooterVelocity() {
return this.currentGlobalShooterVelocity; return this.currentShooterVelocity;
} }
/** /**
* @return The candidate total (robot + shooter) velocity (in field coordinates) * @return The candidate total (robot + shooter) velocity (in field coordinates)
*/ */
public Velocity3d getCandidateGlobalTotalVelocity() { public Velocity3d getCandidateVelocity() {
return this.candidateGlobalTotalVelocity; return this.candidateVelocity;
} }
// //
@ -86,30 +88,25 @@ public class BCEvalParams {
* @return The candidate shooter velocity (in field coordinates) at current * @return The candidate shooter velocity (in field coordinates) at current
* robot velocity * robot velocity
*/ */
public Velocity3d getCandidateGlobalShooterVelocity() { public Velocity3d getCandidateShooterVelocity() {
if(this.cache_candidateGlobalShooterVelocity == null) { if(this.cache_candidateShooterVelocity == null) {
this.cache_candidateGlobalShooterVelocity = this.cache_candidateShooterVelocity =
this.candidateGlobalTotalVelocity this.candidateVelocity
.minus(this.currentGlobalRobotVelocity); .minus(this.robotVelocity);
} }
return this.cache_candidateGlobalShooterVelocity; return this.cache_candidateShooterVelocity;
} }
/** /**
* @return The candidate shooter velocity (in robot coordinates) at current * @return The candidate shooter velocity (in robot coordinates) at current
* robot velocity and heading * robot velocity and heading
*/ */
public Velocity3d getCandidateRelativeShooterVelocity() { public Velocity3d getCandidateShooterVelocityRelative() {
if(this.cache_candidateRelativeShooterVelocity == null) { if(this.cache_candidateShooterVelocityRelative == null) {
this.cache_candidateRelativeShooterVelocity = this.cache_candidateShooterVelocityRelative =
new Velocity3d( this.getCandidateShooterVelocity().rotate(this.robotAngle.times(-1));
this.getCandidateGlobalShooterVelocity().getHorizontalVelocity(),
this.getCandidateGlobalShooterVelocity().getVeritcalVelocity(),
this.getCandidateGlobalShooterVelocity().getAzimuthAngle()
.minus(this.getCurrentGlobalRobotAngle())
);
} }
return this.cache_candidateRelativeShooterVelocity; return this.cache_candidateShooterVelocityRelative;
} }
} }