diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java index 46254a3b12..a3f0017ada 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java @@ -61,6 +61,7 @@ public class LTVDifferentialDriveController { * @param qelems The maximum desired error tolerance for each state. * @param relems The maximum desired control effort for each input. * @param dt Discretization timestep in seconds. + * @throws IllegalArgumentException if max velocity of plant with 12 V input <= 0. */ public LTVDifferentialDriveController( LinearSystem plant, @@ -127,6 +128,11 @@ public class LTVDifferentialDriveController { .times(-1.0) .get(0, 0); + if (maxV <= 0.0) { + throw new IllegalArgumentException( + "Max velocity of plant with 12 V input must be greater than zero."); + } + for (double velocity = -maxV; velocity < maxV; velocity += 0.01) { // The DARE is ill-conditioned if the velocity is close to zero, so don't // let the system stop. 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 701f21bbf4..aa248ebd4f 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 @@ -66,6 +66,7 @@ public class LTVUnicycleController { * @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. */ public LTVUnicycleController(double dt, double maxVelocity) { this(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), dt, maxVelocity); @@ -90,9 +91,14 @@ public class LTVUnicycleController { * @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. */ public LTVUnicycleController( Vector qelems, Vector relems, double dt, double maxVelocity) { + if (maxVelocity <= 0.0) { + throw new IllegalArgumentException("Max velocity must be greater than zero."); + } + // The change in global pose for a unicycle is defined by the following // three equations. // diff --git a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp index 17a0c56a95..7a3a78d4d0 100644 --- a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp +++ b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp @@ -5,6 +5,7 @@ #include "frc/controller/LTVDifferentialDriveController.h" #include +#include #include "frc/MathUtil.h" #include "frc/StateSpaceUtil.h" @@ -65,6 +66,11 @@ LTVDifferentialDriveController::LTVDifferentialDriveController( units::meters_per_second_t maxV{ -plant.A().householderQr().solve(plant.B() * Vectord<2>{12.0, 12.0})(0)}; + if (maxV <= 0_mps) { + throw std::domain_error( + "Max velocity of plant with 12 V input must be greater than zero."); + } + for (auto velocity = -maxV; velocity < maxV; velocity += 0.01_mps) { // The DARE is ill-conditioned if the velocity is close to zero, so don't // let the system stop. diff --git a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp index 256b757192..4f12ac6ac9 100644 --- a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp +++ b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp @@ -4,6 +4,8 @@ #include "frc/controller/LTVUnicycleController.h" +#include + #include "frc/StateSpaceUtil.h" #include "frc/controller/LinearQuadraticRegulator.h" #include "units/math.h" @@ -37,6 +39,10 @@ LTVUnicycleController::LTVUnicycleController( LTVUnicycleController::LTVUnicycleController( const wpi::array& Qelems, const wpi::array& Relems, units::second_t dt, units::meters_per_second_t maxVelocity) { + if (maxVelocity <= 0_mps) { + throw std::domain_error("Max velocity must be greater than zero."); + } + // The change in global pose for a unicycle is defined by the following three // equations. // diff --git a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h index 0a336aa4d0..4ecc15468a 100644 --- a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h +++ b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h @@ -41,6 +41,7 @@ class WPILIB_DLLEXPORT LTVDifferentialDriveController { * @param Qelems The maximum desired error tolerance for each state. * @param Relems The maximum desired control effort for each input. * @param dt Discretization timestep. + * @throws std::domain_error if max velocity of plant with 12 V input <= 0. */ LTVDifferentialDriveController(const frc::LinearSystem<2, 2, 2>& plant, units::meter_t trackwidth, diff --git a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h index 83cfe4bfd2..9647399a58 100644 --- a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h +++ b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h @@ -36,6 +36,7 @@ class WPILIB_DLLEXPORT LTVUnicycleController { * @param dt Discretization timestep. * @param maxVelocity The maximum velocity for the controller gain lookup * table. + * @throws std::domain_error if maxVelocity <= 0. */ explicit LTVUnicycleController( units::second_t dt, units::meters_per_second_t maxVelocity = 9_mps); @@ -48,6 +49,7 @@ class WPILIB_DLLEXPORT LTVUnicycleController { * @param dt Discretization timestep. * @param maxVelocity The maximum velocity for the controller gain lookup * table. + * @throws std::domain_error if maxVelocity <= 0. */ LTVUnicycleController(const wpi::array& Qelems, const wpi::array& Relems, units::second_t dt,