From ad5d57b4973904c8e300d580ebd6842116ca4834 Mon Sep 17 00:00:00 2001 From: sciencewhiz Date: Fri, 27 Dec 2024 19:51:27 -0800 Subject: [PATCH] [wpimath] improve LTVUnicycleController docs (NFC) Document the states and inputs so it isn't necessary to look at the code Fix max velocity throws doc --- .../controller/LTVUnicycleController.java | 23 +++++++++++-------- .../frc/controller/LTVUnicycleController.h | 13 +++++++---- 2 files changed, 22 insertions(+), 14 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java index 8437eafac8e..1757dd666db 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java @@ -54,8 +54,9 @@ private enum State { /** * Constructs a linear time-varying unicycle controller with default maximum desired error - * tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum desired control effort of (1 m/s, - * 2 rad/s). + * tolerances of (x = 0.0625 m, y = 0.125 m, heading = 2 rad), default maximum desired control + * effort of (linear velocity = 1 m/s, angular velocity = 2 rad/s), and default maximum Velocity + * of 9 m/s. * * @param dt Discretization timestep in seconds. */ @@ -65,13 +66,13 @@ public LTVUnicycleController(double dt) { /** * Constructs a linear time-varying unicycle controller with default maximum desired error - * tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum desired control effort of (1 m/s, - * 2 rad/s). + * tolerances of (x = 0.0625 m, y = 0.125 m, heading = 2 rad), default maximum desired control + * effort of (1 m/s, 2 rad/s). * * @param dt Discretization timestep in seconds. * @param maxVelocity The maximum velocity in meters per second for the controller gain lookup * table. The default is 9 m/s. - * @throws IllegalArgumentException if maxVelocity <= 0. + * @throws IllegalArgumentException if maxVelocity <= 0 m/s or >= 15 m/s. */ public LTVUnicycleController(double dt, double maxVelocity) { this(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), dt, maxVelocity); @@ -84,8 +85,11 @@ public LTVUnicycleController(double dt, double maxVelocity) { * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning * for how to select the tolerances. * - * @param qelems The maximum desired error tolerance for each state. - * @param relems The maximum desired control effort for each input. + *

The default maximum Velocity is 9 m/s. + * + * @param qelems The maximum desired error tolerance for each state (x, y, heading). + * @param relems The maximum desired control effort for each input (linear velocity, angular + * velocity). * @param dt Discretization timestep in seconds. */ public LTVUnicycleController(Vector qelems, Vector relems, double dt) { @@ -99,8 +103,9 @@ public LTVUnicycleController(Vector qelems, Vector relems, double dt) { * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning * for how to select the tolerances. * - * @param qelems The maximum desired error tolerance for each state. - * @param relems The maximum desired control effort for each input. + * @param qelems The maximum desired error tolerance for each state (x, y, heading). + * @param relems The maximum desired control effort for each input (linear velocity, angular + * velocity). * @param dt Discretization timestep in seconds. * @param maxVelocity The maximum velocity in meters per second for the controller gain lookup * table. The default is 9 m/s. diff --git a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h index 02f5271ad3e..54e8fe9e3d2 100644 --- a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h +++ b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h @@ -33,13 +33,14 @@ class WPILIB_DLLEXPORT LTVUnicycleController { public: /** * Constructs a linear time-varying unicycle controller with default maximum - * desired error tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum - * desired control effort of (1 m/s, 2 rad/s). + * desired error tolerances of (x = 0.0625 m, y = 0.125 m, heading = 2 rad) + * and default maximum desired control effort of (linear velocity = 1 m/s, + * angular velocity = 2 rad/s). * * @param dt Discretization timestep. * @param maxVelocity The maximum velocity for the controller gain lookup * table. - * @throws std::domain_error if maxVelocity <= 0. + * @throws std::domain_error if maxVelocity <= 0 m/s or >= 15 m/s. */ explicit LTVUnicycleController( units::second_t dt, units::meters_per_second_t maxVelocity = 9_mps); @@ -51,8 +52,10 @@ class WPILIB_DLLEXPORT LTVUnicycleController { * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning * for how to select the tolerances. * - * @param Qelems The maximum desired error tolerance for each state. - * @param Relems The maximum desired control effort for each input. + * @param Qelems The maximum desired error tolerance for each state (x, y, + * heading). + * @param Relems The maximum desired control effort for each input (linear + * velocity, angular velocity). * @param dt Discretization timestep. * @param maxVelocity The maximum velocity for the controller gain lookup * table.