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

[wpimath] Make SimpleMotorFeedforward only support discrete feedforward (#6647)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Nicholas Armstrong
2024-07-16 20:23:11 -04:00
committed by GitHub
parent 5f261a88af
commit 30c7632ab8
31 changed files with 540 additions and 218 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -4,12 +4,14 @@
package edu.wpi.first.math.controller;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.units.MutableMeasure;
import org.junit.jupiter.api.Test;
class SimpleMotorFeedforwardTest {
@@ -24,22 +26,27 @@ class SimpleMotorFeedforwardTest {
var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / Ka);
var plantInversion = new LinearPlantInversionFeedforward<N1, N1, N1>(A, B, dt);
var simpleMotor = new SimpleMotorFeedforward(Ks, Kv, Ka);
var simpleMotor = new SimpleMotorFeedforward(Ks, Kv, Ka, dt);
var r = VecBuilder.fill(2.0);
var nextR = VecBuilder.fill(3.0);
var currentVelocity = MutableMeasure.ofBaseUnits(2.0, RadiansPerSecond);
var nextVelocity = MutableMeasure.ofBaseUnits(3.0, RadiansPerSecond);
assertEquals(37.52499583432516 + 0.5, simpleMotor.calculate(2.0, 3.0, dt), 0.002);
assertEquals(
37.52499583432516 + 0.5,
simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(),
0.002);
assertEquals(
plantInversion.calculate(r, nextR).get(0, 0) + Ks,
simpleMotor.calculate(2.0, 3.0, dt),
simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(),
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(2.0, 1.0 / dt),
simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(),
2.0);
}
}

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

@@ -4,6 +4,8 @@
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;
@@ -41,33 +43,49 @@ class DifferentialDriveVoltageConstraintTest {
point.velocityMetersPerSecond,
0,
point.velocityMetersPerSecond * point.curvatureRadPerMeter);
var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
t += dt;
var acceleration = point.accelerationMetersPerSecondSq;
// Not really a strictly-correct test as we're using the chassis accel instead of the
// wheel accel, but much easier than doing it "properly" and a reasonable check anyway
assertAll(
() ->
assertTrue(
feedforward.calculate(
wheelSpeeds.leftMetersPerSecond, point.accelerationMetersPerSecondSq)
feedforward
.calculate(
MetersPerSecond.of(wheelSpeeds.leftMetersPerSecond),
MetersPerSecond.of(
wheelSpeeds.leftMetersPerSecond + dt * acceleration))
.in(Volts)
<= maxVoltage + 0.05),
() ->
assertTrue(
feedforward.calculate(
wheelSpeeds.leftMetersPerSecond, point.accelerationMetersPerSecondSq)
feedforward
.calculate(
MetersPerSecond.of(wheelSpeeds.leftMetersPerSecond),
MetersPerSecond.of(
wheelSpeeds.leftMetersPerSecond + dt * acceleration))
.in(Volts)
>= -maxVoltage - 0.05),
() ->
assertTrue(
feedforward.calculate(
wheelSpeeds.rightMetersPerSecond, point.accelerationMetersPerSecondSq)
feedforward
.calculate(
MetersPerSecond.of(wheelSpeeds.rightMetersPerSecond),
MetersPerSecond.of(
wheelSpeeds.rightMetersPerSecond + dt * acceleration))
.in(Volts)
<= maxVoltage + 0.05),
() ->
assertTrue(
feedforward.calculate(
wheelSpeeds.rightMetersPerSecond, point.accelerationMetersPerSecondSq)
feedforward
.calculate(
MetersPerSecond.of(wheelSpeeds.rightMetersPerSecond),
MetersPerSecond.of(
wheelSpeeds.rightMetersPerSecond + dt * acceleration))
.in(Volts)
>= -maxVoltage - 0.05));
}
}

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

@@ -4,19 +4,21 @@
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;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.units.MutableMeasure;
import java.util.List;
import org.junit.jupiter.api.Test;
class ExponentialProfileTest {
private static final double kDt = 0.01;
private static final SimpleMotorFeedforward feedforward =
new SimpleMotorFeedforward(0, 2.5629, 0.43277);
new SimpleMotorFeedforward(0, 2.5629, 0.43277, kDt);
private static final ExponentialProfile.Constraints constraints =
ExponentialProfile.Constraints.fromCharacteristics(12, 2.5629, 0.43277);
@@ -43,10 +45,11 @@ class ExponentialProfileTest {
private static ExponentialProfile.State checkDynamics(
ExponentialProfile profile, ExponentialProfile.State current, ExponentialProfile.State goal) {
var next = profile.calculate(kDt, current, goal);
var currentVelocity = MutableMeasure.ofBaseUnits(current.velocity, RadiansPerSecond);
var nextVelocity = MutableMeasure.ofBaseUnits(next.velocity, RadiansPerSecond);
var signal = feedforward.calculate(currentVelocity, nextVelocity);
var signal = feedforward.calculate(current.velocity, next.velocity, kDt);
assertTrue(Math.abs(signal) < constraints.maxInput + 1e-9);
assertTrue(Math.abs(signal.magnitude()) < constraints.maxInput + 1e-9);
return next;
}