From d5200db6cd5cc32d44f4433be2cac2b6d8b9d47a Mon Sep 17 00:00:00 2001 From: Starlight220 <53231611+Starlight220@users.noreply.github.com> Date: Thu, 24 Nov 2022 09:13:50 +0200 Subject: [PATCH] [wpimath] Rename HolonomicDriveController.calculate params (#4683) --- .../controller/HolonomicDriveController.java | 38 ++++++++++--------- .../controller/HolonomicDriveController.cpp | 27 ++++++------- .../frc/controller/HolonomicDriveController.h | 36 +++++++++--------- 3 files changed, 53 insertions(+), 48 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java index 5b3bd91d9a..e43c6ff3d4 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java +++ b/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 poseRef The desired pose. - * @param linearVelocityRefMeters The linear velocity reference. - * @param angleRef The angular reference. + * @param currentPose The current pose, as measured by odometry or pose estimator. + * @param trajectoryPose The desired trajectory pose, as sampled for the current timestep. + * @param desiredLinearVelocityMetersPerSecond The desired linear velocity. + * @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 yFF = linearVelocityRefMeters * poseRef.getRotation().getSin(); + double xFF = desiredLinearVelocityMetersPerSecond * trajectoryPose.getRotation().getCos(); + 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_rotationError = angleRef.minus(currentPose.getRotation()); + m_poseError = trajectoryPose.relativeTo(currentPose); + 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 yFeedback = m_yController.calculate(currentPose.getY(), poseRef.getY()); + double xFeedback = m_xController.calculate(currentPose.getX(), trajectoryPose.getX()); + 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 desiredState The desired trajectory state. - * @param angleRef The desired end-angle. + * @param currentPose The current pose, as measured by odometry or pose estimator. + * @param desiredState The desired trajectory pose, as sampled for the current timestep. + * @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); } /** diff --git a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp index de0414778e..def723470f 100644 --- a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp +++ b/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, - units::meters_per_second_t linearVelocityRef, const Rotation2d& angleRef) { + const Pose2d& currentPose, const Pose2d& trajectoryPose, + 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 yFF = linearVelocityRef * poseRef.Rotation().Sin(); + auto xFF = desiredLinearVelocity * trajectoryPose.Rotation().Cos(); + 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_rotationError = angleRef - currentPose.Rotation(); + m_poseError = trajectoryPose.RelativeTo(currentPose); + 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{ - m_xController.Calculate(currentPose.X().value(), poseRef.X().value())}; - auto yFeedback = units::meters_per_second_t{ - m_yController.Calculate(currentPose.Y().value(), poseRef.Y().value())}; + auto xFeedback = units::meters_per_second_t{m_xController.Calculate( + currentPose.X().value(), trajectoryPose.X().value())}; + auto yFeedback = units::meters_per_second_t{m_yController.Calculate( + 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) { diff --git a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h index 398a872bad..93aa58e704 100644 --- a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h +++ b/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 - * a drivetrain trajectory. - * - * @param currentPose The current pose. - * @param poseRef The desired pose. - * @param linearVelocityRef The desired linear velocity. - * @param angleRef The desired ending angle. + * @param currentPose The current pose, as measured by odometry or pose + * estimator. + * @param trajectoryPose The desired trajectory pose, as sampled for the + * current timestep. + * @param desiredLinearVelocity The desired linear velocity. + * @param desiredHeading The desired heading. + * @return The next output of the holonomic drive controller. */ - ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef, - units::meters_per_second_t linearVelocityRef, - const Rotation2d& angleRef); + ChassisSpeeds Calculate(const Pose2d& currentPose, + const Pose2d& trajectoryPose, + 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 - * a drivetrain trajectory. - * - * @param currentPose The current pose. - * @param desiredState The desired pose, linear velocity, and angular velocity - * from a trajectory. - * @param angleRef The desired ending angle. + * @param currentPose The current pose, as measured by odometry or pose + * estimator. + * @param desiredState The desired trajectory pose, as sampled for the current + * timestep. + * @param desiredHeading The desired heading. + * @return The next output of the holonomic drive controller. */ ChassisSpeeds Calculate(const Pose2d& currentPose, const Trajectory::State& desiredState, - const Rotation2d& angleRef); + const Rotation2d& desiredHeading); /** * Enables and disables the controller for troubleshooting purposes. When