Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-05 03:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2024-12-07 14:11:47 -08:00
parent c8900cadc3 f772bb141d
commit 52b33edcbd
44 changed files with 555 additions and 1044 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java
View File

@@ -4,7 +4,6 @@
package edu.wpi.first.math.controller;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertThrows;
@@ -31,23 +30,23 @@ class SimpleMotorFeedforwardTest {
var r = VecBuilder.fill(2.0);
var nextR = VecBuilder.fill(3.0);
var currentVelocity = RadiansPerSecond.mutable(2.0);
var nextVelocity = RadiansPerSecond.mutable(3.0);
double currentVelocity = 2.0; // rad/s
double nextVelocity = 3.0; // rad/s
assertEquals(
37.52499583432516 + 0.5,
simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(),
simpleMotor.calculateWithVelocities(currentVelocity, nextVelocity),
0.002);
assertEquals(
plantInversion.calculate(r, nextR).get(0, 0) + Ks,
simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(),
simpleMotor.calculateWithVelocities(currentVelocity, nextVelocity),
0.002);
// These won't match exactly. It's just an approximation to make sure they're
// in the same ballpark.
assertEquals(
plantInversion.calculate(r, nextR).get(0, 0) + Ks,
simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(),
simpleMotor.calculateWithVelocities(currentVelocity, nextVelocity),
2.0);
}

38
wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java
View File

@@ -4,8 +4,6 @@
package edu.wpi.first.math.trajectory;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertTrue;
@@ -53,39 +51,27 @@ class DifferentialDriveVoltageConstraintTest {
assertAll(
() ->
assertTrue(
feedforward
.calculate(
MetersPerSecond.of(wheelSpeeds.leftMetersPerSecond),
MetersPerSecond.of(
wheelSpeeds.leftMetersPerSecond + dt * acceleration))
.in(Volts)
feedforward.calculateWithVelocities(
wheelSpeeds.leftMetersPerSecond,
wheelSpeeds.leftMetersPerSecond + dt * acceleration)
<= maxVoltage + 0.05),
() ->
assertTrue(
feedforward
.calculate(
MetersPerSecond.of(wheelSpeeds.leftMetersPerSecond),
MetersPerSecond.of(
wheelSpeeds.leftMetersPerSecond + dt * acceleration))
.in(Volts)
feedforward.calculateWithVelocities(
wheelSpeeds.leftMetersPerSecond,
wheelSpeeds.leftMetersPerSecond + dt * acceleration)
>= -maxVoltage - 0.05),
() ->
assertTrue(
feedforward
.calculate(
MetersPerSecond.of(wheelSpeeds.rightMetersPerSecond),
MetersPerSecond.of(
wheelSpeeds.rightMetersPerSecond + dt * acceleration))
.in(Volts)
feedforward.calculateWithVelocities(
wheelSpeeds.rightMetersPerSecond,
wheelSpeeds.rightMetersPerSecond + dt * acceleration)
<= maxVoltage + 0.05),
() ->
assertTrue(
feedforward
.calculate(
MetersPerSecond.of(wheelSpeeds.rightMetersPerSecond),
MetersPerSecond.of(
wheelSpeeds.rightMetersPerSecond + dt * acceleration))
.in(Volts)
feedforward.calculateWithVelocities(
wheelSpeeds.rightMetersPerSecond,
wheelSpeeds.rightMetersPerSecond + dt * acceleration)
>= -maxVoltage - 0.05));
}
}

7
wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java
View File

@@ -4,7 +4,6 @@
package edu.wpi.first.math.trajectory;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
@@ -44,11 +43,9 @@ class ExponentialProfileTest {
private static ExponentialProfile.State checkDynamics(
ExponentialProfile profile, ExponentialProfile.State current, ExponentialProfile.State goal) {
var next = profile.calculate(kDt, current, goal);
var currentVelocity = RadiansPerSecond.mutable(current.velocity);
var nextVelocity = RadiansPerSecond.mutable(next.velocity);
var signal = feedforward.calculate(currentVelocity, nextVelocity);
var signal = feedforward.calculateWithVelocities(current.velocity, next.velocity);
assertTrue(Math.abs(signal.magnitude()) < constraints.maxInput + 1e-9);
assertTrue(Math.abs(signal) < constraints.maxInput + 1e-9);
return next;
}