Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +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/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
View File

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

6
wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
View File

@@ -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 &lt;= 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 &lt;= 0.
*/
public LTVUnicycleController(
Vector<N3> qelems, Vector<N2> 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.
//