[wpimath] Check LTV controller max velocity precondition (#5142)

This commit is contained in:
Tyler Veness
2023-02-26 15:05:41 -08:00
committed by GitHub
parent 4b0eecaee0
commit ce3686b80d
6 changed files with 27 additions and 0 deletions

View File

@@ -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.