[wpimath] Rename HolonomicDriveController.calculate params (#4683)

wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java

@@ -75,14 +75,17 @@ public class HolonomicDriveController {
   /**
    * Returns the next output of the holonomic drive controller.
    *
-   * @param currentPose The current pose.
+   * @param currentPose The current pose, as measured by odometry or pose estimator.
-   * @param poseRef The desired pose.
+   * @param trajectoryPose The desired trajectory pose, as sampled for the current timestep.
-   * @param linearVelocityRefMeters The linear velocity reference.
+   * @param desiredLinearVelocityMetersPerSecond The desired linear velocity.
-   * @param angleRef The angular reference.
+   * @param desiredHeading The desired heading.
    * @return The next output of the holonomic drive controller.
    */
   public ChassisSpeeds calculate(
-      Pose2d currentPose, Pose2d poseRef, double linearVelocityRefMeters, Rotation2d angleRef) {
+      Pose2d currentPose,
+      Pose2d trajectoryPose,
+      double desiredLinearVelocityMetersPerSecond,
+      Rotation2d desiredHeading) {
     // If this is the first run, then we need to reset the theta controller to the current pose's
     // heading.
     if (m_firstRun) {
@@ -91,21 +94,22 @@ public class HolonomicDriveController {
     }
 
     // Calculate feedforward velocities (field-relative).
-    double xFF = linearVelocityRefMeters * poseRef.getRotation().getCos();
+    double xFF = desiredLinearVelocityMetersPerSecond * trajectoryPose.getRotation().getCos();
-    double yFF = linearVelocityRefMeters * poseRef.getRotation().getSin();
+    double yFF = desiredLinearVelocityMetersPerSecond * trajectoryPose.getRotation().getSin();
     double thetaFF =
-        m_thetaController.calculate(currentPose.getRotation().getRadians(), angleRef.getRadians());
+        m_thetaController.calculate(
+            currentPose.getRotation().getRadians(), desiredHeading.getRadians());
 
-    m_poseError = poseRef.relativeTo(currentPose);
+    m_poseError = trajectoryPose.relativeTo(currentPose);
-    m_rotationError = angleRef.minus(currentPose.getRotation());
+    m_rotationError = desiredHeading.minus(currentPose.getRotation());
 
     if (!m_enabled) {
       return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, thetaFF, currentPose.getRotation());
     }
 
     // Calculate feedback velocities (based on position error).
-    double xFeedback = m_xController.calculate(currentPose.getX(), poseRef.getX());
+    double xFeedback = m_xController.calculate(currentPose.getX(), trajectoryPose.getX());
-    double yFeedback = m_yController.calculate(currentPose.getY(), poseRef.getY());
+    double yFeedback = m_yController.calculate(currentPose.getY(), trajectoryPose.getY());
 
     // Return next output.
     return ChassisSpeeds.fromFieldRelativeSpeeds(
@@ -115,15 +119,15 @@ public class HolonomicDriveController {
   /**
    * Returns the next output of the holonomic drive controller.
    *
-   * @param currentPose The current pose.
+   * @param currentPose The current pose, as measured by odometry or pose estimator.
-   * @param desiredState The desired trajectory state.
+   * @param desiredState The desired trajectory pose, as sampled for the current timestep.
-   * @param angleRef The desired end-angle.
+   * @param desiredHeading The desired heading.
    * @return The next output of the holonomic drive controller.
    */
   public ChassisSpeeds calculate(
-      Pose2d currentPose, Trajectory.State desiredState, Rotation2d angleRef) {
+      Pose2d currentPose, Trajectory.State desiredState, Rotation2d desiredHeading) {
     return calculate(
-        currentPose, desiredState.poseMeters, desiredState.velocityMetersPerSecond, angleRef);
+        currentPose, desiredState.poseMeters, desiredState.velocityMetersPerSecond, desiredHeading);
   }
 
   /**

wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp

@@ -34,8 +34,9 @@ void HolonomicDriveController::SetTolerance(const Pose2d& tolerance) {
 }
 
 ChassisSpeeds HolonomicDriveController::Calculate(
-    const Pose2d& currentPose, const Pose2d& poseRef,
+    const Pose2d& currentPose, const Pose2d& trajectoryPose,
-    units::meters_per_second_t linearVelocityRef, const Rotation2d& angleRef) {
+    units::meters_per_second_t desiredLinearVelocity,
+    const Rotation2d& desiredHeading) {
   // If this is the first run, then we need to reset the theta controller to the
   // current pose's heading.
   if (m_firstRun) {
@@ -44,13 +45,13 @@ ChassisSpeeds HolonomicDriveController::Calculate(
   }
 
   // Calculate feedforward velocities (field-relative)
-  auto xFF = linearVelocityRef * poseRef.Rotation().Cos();
+  auto xFF = desiredLinearVelocity * trajectoryPose.Rotation().Cos();
-  auto yFF = linearVelocityRef * poseRef.Rotation().Sin();
+  auto yFF = desiredLinearVelocity * trajectoryPose.Rotation().Sin();
   auto thetaFF = units::radians_per_second_t{m_thetaController.Calculate(
-      currentPose.Rotation().Radians(), angleRef.Radians())};
+      currentPose.Rotation().Radians(), desiredHeading.Radians())};
 
-  m_poseError = poseRef.RelativeTo(currentPose);
+  m_poseError = trajectoryPose.RelativeTo(currentPose);
-  m_rotationError = angleRef - currentPose.Rotation();
+  m_rotationError = desiredHeading - currentPose.Rotation();
 
   if (!m_enabled) {
     return ChassisSpeeds::FromFieldRelativeSpeeds(xFF, yFF, thetaFF,
@@ -58,10 +59,10 @@ ChassisSpeeds HolonomicDriveController::Calculate(
   }
 
   // Calculate feedback velocities (based on position error).
-  auto xFeedback = units::meters_per_second_t{
+  auto xFeedback = units::meters_per_second_t{m_xController.Calculate(
-      m_xController.Calculate(currentPose.X().value(), poseRef.X().value())};
+      currentPose.X().value(), trajectoryPose.X().value())};
-  auto yFeedback = units::meters_per_second_t{
+  auto yFeedback = units::meters_per_second_t{m_yController.Calculate(
-      m_yController.Calculate(currentPose.Y().value(), poseRef.Y().value())};
+      currentPose.Y().value(), trajectoryPose.Y().value())};
 
   // Return next output.
   return ChassisSpeeds::FromFieldRelativeSpeeds(
@@ -70,9 +71,9 @@ ChassisSpeeds HolonomicDriveController::Calculate(
 
 ChassisSpeeds HolonomicDriveController::Calculate(
     const Pose2d& currentPose, const Trajectory::State& desiredState,
-    const Rotation2d& angleRef) {
+    const Rotation2d& desiredHeading) {
   return Calculate(currentPose, desiredState.pose, desiredState.velocity,
-                   angleRef);
+                   desiredHeading);
 }
 
 void HolonomicDriveController::SetEnabled(bool enabled) {

wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h

@@ -61,32 +61,32 @@ class WPILIB_DLLEXPORT HolonomicDriveController {
   /**
    * Returns the next output of the holonomic drive controller.
    *
-   * The reference pose, linear velocity, and angular velocity should come from
+   * @param currentPose The current pose, as measured by odometry or pose
-   * a drivetrain trajectory.
+   * estimator.
-   *
+   * @param trajectoryPose The desired trajectory pose, as sampled for the
-   * @param currentPose        The current pose.
+   * current timestep.
-   * @param poseRef            The desired pose.
+   * @param desiredLinearVelocity The desired linear velocity.
-   * @param linearVelocityRef  The desired linear velocity.
+   * @param desiredHeading The desired heading.
-   * @param angleRef           The desired ending angle.
+   * @return The next output of the holonomic drive controller.
    */
-  ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
+  ChassisSpeeds Calculate(const Pose2d& currentPose,
-                          units::meters_per_second_t linearVelocityRef,
+                          const Pose2d& trajectoryPose,
-                          const Rotation2d& angleRef);
+                          units::meters_per_second_t desiredLinearVelocity,
+                          const Rotation2d& desiredHeading);
 
   /**
    * Returns the next output of the holonomic drive controller.
    *
-   * The reference pose, linear velocity, and angular velocity should come from
+   * @param currentPose The current pose, as measured by odometry or pose
-   * a drivetrain trajectory.
+   * estimator.
-   *
+   * @param desiredState The desired trajectory pose, as sampled for the current
-   * @param currentPose  The current pose.
+   * timestep.
-   * @param desiredState The desired pose, linear velocity, and angular velocity
+   * @param desiredHeading The desired heading.
-   *                     from a trajectory.
+   * @return The next output of the holonomic drive controller.
-   * @param angleRef     The desired ending angle.
    */
   ChassisSpeeds Calculate(const Pose2d& currentPose,
                           const Trajectory::State& desiredState,
-                          const Rotation2d& angleRef);
+                          const Rotation2d& desiredHeading);
 
   /**
    * Enables and disables the controller for troubleshooting purposes. When