Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-26 01:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files

6
wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp
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.

6
wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
View File

@@ -4,6 +4,8 @@
#include "frc/controller/LTVUnicycleController.h"
#include <stdexcept>
#include "frc/StateSpaceUtil.h"
#include "frc/controller/LinearQuadraticRegulator.h"
#include "units/math.h"
@@ -37,6 +39,10 @@ LTVUnicycleController::LTVUnicycleController(
LTVUnicycleController::LTVUnicycleController(
const wpi::array<double, 3>& Qelems, const wpi::array<double, 2>& 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.
//

1
wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h
View File

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

2
wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h
View File

@@ -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 &lt;= 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 &lt;= 0.
*/
LTVUnicycleController(const wpi::array<double, 3>& Qelems,
const wpi::array<double, 2>& Relems, units::second_t dt,