mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
[wpimath] Check LTV controller max velocity precondition (#5142)
This commit is contained in:
@@ -5,6 +5,7 @@
|
||||
#include "frc/controller/LTVDifferentialDriveController.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <stdexcept>
|
||||
|
||||
#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.
|
||||
|
||||
Reference in New Issue
Block a user