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

[wpimath] Add minLinearAccel parameter to DifferentialDriveAccelerationLimiter (#4422)

Browse Source
This commit is contained in:
Ryan Blue
2022-10-10 11:57:37 -04:00
committed by GitHub
parent 2a13dba8ac
commit 27b173374e
5 changed files with 246 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

96
wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java
View File

@@ -4,7 +4,9 @@
package edu.wpi.first.math.controller;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertThrows;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.MatBuilder;
@@ -204,4 +206,98 @@ class DifferentialDriveAccelerationLimiterTest {
assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5);
}
}
@Test
void testSeperateMinMaxLowLimits() {
final double trackwidth = 0.9;
final double dt = 0.005;
final double minA = -1.0;
final double maxA = 2.0;
final double maxAlpha = 2.0;
var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
var accelLimiter =
new DifferentialDriveAccelerationLimiter(plant, trackwidth, minA, maxA, maxAlpha);
var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
// Ensure voltage exceeds acceleration before limiting
{
final var accels =
plant
.getA()
.times(xAccelLimiter)
.plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0)));
final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
System.out.println(a);
assertTrue(Math.abs(a) > maxA);
assertTrue(Math.abs(a) > -minA);
}
// a should always be within [minA, maxA]
// Forward
var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
final var voltages =
accelLimiter.calculate(
xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
xAccelLimiter =
plant.calculateX(
xAccelLimiter,
new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
dt);
final var accels =
plant
.getA()
.times(xAccelLimiter)
.plus(
plant
.getB()
.times(
new MatBuilder<>(Nat.N2(), Nat.N1())
.fill(voltages.left, voltages.right)));
final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
assertTrue(minA <= a && a <= maxA);
}
// Backward
u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
final var voltages =
accelLimiter.calculate(
xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
xAccelLimiter =
plant.calculateX(
xAccelLimiter,
new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
dt);
final var accels =
plant
.getA()
.times(xAccelLimiter)
.plus(
plant
.getB()
.times(
new MatBuilder<>(Nat.N2(), Nat.N1())
.fill(voltages.left, voltages.right)));
final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
assertTrue(minA <= a && a <= maxA);
}
}
@Test
void testMinAccelGreaterThanMaxAccel() {
var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
assertDoesNotThrow(() -> new DifferentialDriveAccelerationLimiter(plant, 1, -1, 1, 1e3));
assertThrows(
IllegalArgumentException.class,
() -> new DifferentialDriveAccelerationLimiter(plant, 1, 1, -1, 1e3));
}
}