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 866d062388..c30b200792 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 @@ -65,7 +65,8 @@ 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. + * @throws IllegalArgumentException if max velocity of plant with 12 V input <= 0 m/s or >= + * 15 m/s. */ public LTVDifferentialDriveController( LinearSystem plant, @@ -134,7 +135,11 @@ public class LTVDifferentialDriveController { if (maxV <= 0.0) { throw new IllegalArgumentException( - "Max velocity of plant with 12 V input must be greater than zero."); + "Max velocity of plant with 12 V input must be greater than 0 m/s."); + } + if (maxV >= 15.0) { + throw new IllegalArgumentException( + "Max velocity of plant with 12 V input must be less than 15 m/s."); } for (double velocity = -maxV; velocity < maxV; velocity += 0.01) { 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 b0a18df6f1..d2ac72b7c3 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 @@ -99,12 +99,15 @@ 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. + * @throws IllegalArgumentException if maxVelocity <= 0 m/s or >= 15 m/s. */ public LTVUnicycleController( Vector qelems, Vector relems, double dt, double maxVelocity) { if (maxVelocity <= 0.0) { - throw new IllegalArgumentException("Max velocity must be greater than zero."); + throw new IllegalArgumentException("Max velocity must be greater than 0 m/s."); + } + if (maxVelocity >= 15.0) { + throw new IllegalArgumentException("Max velocity must be less than 15 m/s."); } // The change in global pose for a unicycle is defined by the following diff --git a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp index 7a3a78d4d0..7d32bfdebd 100644 --- a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp +++ b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp @@ -68,7 +68,11 @@ LTVDifferentialDriveController::LTVDifferentialDriveController( if (maxV <= 0_mps) { throw std::domain_error( - "Max velocity of plant with 12 V input must be greater than zero."); + "Max velocity of plant with 12 V input must be greater than 0 m/s."); + } + if (maxV >= 15_mps) { + throw std::domain_error( + "Max velocity of plant with 12 V input must be less than 15 m/s."); } for (auto velocity = -maxV; velocity < maxV; velocity += 0.01_mps) { diff --git a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp index 4f12ac6ac9..75188cb7a8 100644 --- a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp +++ b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp @@ -40,7 +40,10 @@ 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."); + throw std::domain_error("Max velocity must be greater than 0 m/s."); + } + if (maxVelocity >= 15_mps) { + throw std::domain_error("Max velocity must be less than 15 m/s."); } // The change in global pose for a unicycle is defined by the following three diff --git a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h index 7c85350608..3995d3a3a7 100644 --- a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h +++ b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h @@ -45,7 +45,8 @@ 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. + * @throws std::domain_error if max velocity of plant with 12 V input <= 0 m/s + * or >= 15 m/s. */ 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 9ea4606b14..b128dbb7d4 100644 --- a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h +++ b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h @@ -53,7 +53,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. + * @throws std::domain_error if maxVelocity <= 0 m/s or >= 15 m/s. */ LTVUnicycleController(const wpi::array& Qelems, const wpi::array& Relems, units::second_t dt,