Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[wpimath] Fix SimpleFeedforward overload set #7516

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -368,19 +368,16 @@ public void execute() {

if (m_usePID) {
final double frontLeftFeedforward =
m_feedforward.calculateWithVelocities(
m_prevFrontLeftSpeedSetpoint, frontLeftSpeedSetpoint);
m_feedforward.calculate(m_prevFrontLeftSpeedSetpoint, frontLeftSpeedSetpoint);

final double rearLeftFeedforward =
m_feedforward.calculateWithVelocities(m_prevRearLeftSpeedSetpoint, rearLeftSpeedSetpoint);
m_feedforward.calculate(m_prevRearLeftSpeedSetpoint, rearLeftSpeedSetpoint);

final double frontRightFeedforward =
m_feedforward.calculateWithVelocities(
m_prevFrontRightSpeedSetpoint, frontRightSpeedSetpoint);
m_feedforward.calculate(m_prevFrontRightSpeedSetpoint, frontRightSpeedSetpoint);

final double rearRightFeedforward =
m_feedforward.calculateWithVelocities(
m_prevRearRightSpeedSetpoint, rearRightSpeedSetpoint);
m_feedforward.calculate(m_prevRearRightSpeedSetpoint, rearRightSpeedSetpoint);

frontLeftOutput =
frontLeftFeedforward
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -181,11 +181,10 @@ public void execute() {
double rightOutput;

if (m_usePID) {
double leftFeedforward =
m_feedforward.calculateWithVelocities(m_prevLeftSpeedSetpoint, leftSpeedSetpoint);
double leftFeedforward = m_feedforward.calculate(m_prevLeftSpeedSetpoint, leftSpeedSetpoint);

double rightFeedforward =
m_feedforward.calculateWithVelocities(m_prevRightSpeedSetpoint, rightSpeedSetpoint);
m_feedforward.calculate(m_prevRightSpeedSetpoint, rightSpeedSetpoint);

leftOutput =
leftFeedforward
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -96,12 +96,12 @@ public void setDriveStates(
m_leftLeader.setSetpoint(
ExampleSmartMotorController.PIDMode.kPosition,
currentLeft.position,
m_feedforward.calculateWithVelocities(currentLeft.velocity, nextLeft.velocity)
m_feedforward.calculate(currentLeft.velocity, nextLeft.velocity)
/ RobotController.getBatteryVoltage());
m_rightLeader.setSetpoint(
ExampleSmartMotorController.PIDMode.kPosition,
currentRight.position,
m_feedforward.calculateWithVelocities(currentLeft.velocity, nextLeft.velocity)
m_feedforward.calculate(currentLeft.velocity, nextLeft.velocity)
/ RobotController.getBatteryVoltage());
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -123,20 +123,6 @@ public double getDt() {
return m_dt;
}

/**
* Calculates the feedforward from the gains and setpoints assuming continuous control.
*
* @param velocity The velocity setpoint.
* @param acceleration The acceleration setpoint.
* @return The computed feedforward.
* @deprecated Use {@link #calculateWithVelocities(double, double)} instead.
*/
@SuppressWarnings("removal")
@Deprecated(forRemoval = true, since = "2025")
public double calculate(double velocity, double acceleration) {
return ks * Math.signum(velocity) + kv * velocity + ka * acceleration;
}

/**
* Calculates the feedforward from the gains and velocity setpoint assuming continuous control
* (acceleration is assumed to be zero).
Expand All @@ -157,7 +143,7 @@ public double calculate(double velocity) {
* @param nextVelocity The next velocity setpoint.
* @return The computed feedforward.
*/
public double calculateWithVelocities(double currentVelocity, double nextVelocity) {
public double calculate(double currentVelocity, double nextVelocity) {
// See wpimath/algorithms.md#Simple_motor_feedforward for derivation
if (ka == 0.0) {
return ks * Math.signum(nextVelocity) + kv * nextVelocity;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -67,22 +67,6 @@ class SimpleMotorFeedforward {
}
}

/**
* Calculates the feedforward from the gains and setpoints assuming continuous
* control.
*
* @param velocity The velocity setpoint.
* @param acceleration The acceleration setpoint.
* @return The computed feedforward, in volts.
* @deprecated Use the current/next velocity overload instead.
*/
[[deprecated("Use the current/next velocity overload instead.")]]
constexpr units::volt_t Calculate(
units::unit_t<Velocity> velocity,
units::unit_t<Acceleration> acceleration) const {
return kS * wpi::sgn(velocity) + kV * velocity + kA * acceleration;
}

/**
* Calculates the feedforward from the gains and velocity setpoint assuming
* discrete control. Use this method when the velocity setpoint does not
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,19 +34,17 @@ void testCalculate() {
double nextVelocity = 3.0; // rad/s

assertEquals(
37.52499583432516 + 0.5,
simpleMotor.calculateWithVelocities(currentVelocity, nextVelocity),
0.002);
37.52499583432516 + 0.5, simpleMotor.calculate(currentVelocity, nextVelocity), 0.002);
assertEquals(
plantInversion.calculate(r, nextR).get(0, 0) + Ks,
simpleMotor.calculateWithVelocities(currentVelocity, nextVelocity),
simpleMotor.calculate(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.calculateWithVelocities(currentVelocity, nextVelocity),
simpleMotor.calculate(currentVelocity, nextVelocity),
2.0);
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -51,25 +51,25 @@ void testDifferentialDriveVoltageConstraint() {
assertAll(
() ->
assertTrue(
feedforward.calculateWithVelocities(
feedforward.calculate(
wheelSpeeds.leftMetersPerSecond,
wheelSpeeds.leftMetersPerSecond + dt * acceleration)
<= maxVoltage + 0.05),
() ->
assertTrue(
feedforward.calculateWithVelocities(
feedforward.calculate(
wheelSpeeds.leftMetersPerSecond,
wheelSpeeds.leftMetersPerSecond + dt * acceleration)
>= -maxVoltage - 0.05),
() ->
assertTrue(
feedforward.calculateWithVelocities(
feedforward.calculate(
wheelSpeeds.rightMetersPerSecond,
wheelSpeeds.rightMetersPerSecond + dt * acceleration)
<= maxVoltage + 0.05),
() ->
assertTrue(
feedforward.calculateWithVelocities(
feedforward.calculate(
wheelSpeeds.rightMetersPerSecond,
wheelSpeeds.rightMetersPerSecond + dt * acceleration)
>= -maxVoltage - 0.05));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ private static void assertNear(
private static ExponentialProfile.State checkDynamics(
ExponentialProfile profile, ExponentialProfile.State current, ExponentialProfile.State goal) {
var next = profile.calculate(kDt, current, goal);
var signal = feedforward.calculateWithVelocities(current.velocity, next.velocity);
var signal = feedforward.calculate(current.velocity, next.velocity);

assertTrue(Math.abs(signal) < constraints.maxInput + 1e-9);

Expand Down
Loading