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

[wpimath] Remove Units class from Elevator and Arm Feedforwards (#7570)

Browse Source
This commit is contained in:
Nicholas Armstrong
2024-12-26 21:21:19 -05:00
committed by GitHub
parent 0470e51569
commit fe49cbe429
7 changed files with 26 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java
View File

@@ -4,9 +4,6 @@
package edu.wpi.first.wpilibj.examples.elevatorexponentialsimulation.subsystems;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.ElevatorFeedforward;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.system.plant.DCMotor;
@@ -114,9 +111,7 @@ public class Elevator implements AutoCloseable {
// With the setpoint value we run PID control like normal
double pidOutput = m_pidController.calculate(m_encoder.getDistance(), m_setpoint.position);
double feedforwardOutput =
m_feedforward
.calculate(MetersPerSecond.of(m_setpoint.velocity), MetersPerSecond.of(next.velocity))
.in(Volts);
m_feedforward.calculateWithVelocities(m_setpoint.velocity, next.velocity);
m_motor.setVoltage(pidOutput + feedforwardOutput);

7
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
View File

@@ -4,9 +4,6 @@
package edu.wpi.first.wpilibj.examples.elevatorprofiledpid;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.ElevatorFeedforward;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
@@ -54,8 +51,6 @@ public class Robot extends TimedRobot {
// Run controller and update motor output
m_motor.setVoltage(
m_controller.calculate(m_encoder.getDistance())
+ m_feedforward
.calculate(MetersPerSecond.of(m_controller.getSetpoint().velocity))
.in(Volts));
+ m_feedforward.calculate(m_controller.getSetpoint().velocity));
}
}

6
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java
View File

@@ -4,9 +4,6 @@
package edu.wpi.first.wpilibj.examples.elevatorsimulation.subsystems;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.ElevatorFeedforward;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.system.plant.DCMotor;
@@ -104,8 +101,7 @@ public class Elevator implements AutoCloseable {
// With the setpoint value we run PID control like normal
double pidOutput = m_controller.calculate(m_encoder.getDistance());
double feedforwardOutput =
m_feedforward.calculate(MetersPerSecond.of(m_controller.getSetpoint().velocity)).in(Volts);
double feedforwardOutput = m_feedforward.calculate(m_controller.getSetpoint().velocity);
m_motor.setVoltage(pidOutput + feedforwardOutput);
}

52
wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
View File

@@ -4,16 +4,9 @@
package edu.wpi.first.math.controller;
import static edu.wpi.first.units.Units.Radians;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.proto.ArmFeedforwardProto;
import edu.wpi.first.math.controller.struct.ArmFeedforwardStruct;
import edu.wpi.first.math.jni.ArmFeedforwardJNI;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Voltage;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
@@ -167,8 +160,6 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
* @param velocity The velocity setpoint.
* @return The computed feedforward.
*/
@SuppressWarnings("removal")
@Deprecated(forRemoval = true, since = "2025")
public double calculate(double positionRadians, double velocity) {
return calculate(positionRadians, velocity, 0);
}
@@ -192,45 +183,20 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
ks, kv, ka, kg, currentAngle, currentVelocity, nextVelocity, dt);
}
/**
* Calculates the feedforward from the gains and setpoints assuming discrete control when the
* velocity does not change.
*
* @param currentAngle The current angle. This angle should be measured from the horizontal (i.e.
* if the provided angle is 0, the arm should be parallel to the floor). If your encoder does
* not follow this convention, an offset should be added.
* @param currentVelocity The current velocity.
* @return The computed feedforward in volts.
*/
public Voltage calculate(Angle currentAngle, AngularVelocity currentVelocity) {
return Volts.of(
kg * Math.cos(currentAngle.in(Radians))
+ ks * Math.signum(currentVelocity.in(RadiansPerSecond))
+ kv * currentVelocity.in(RadiansPerSecond));
}
/**
* Calculates the feedforward from the gains and setpoints assuming discrete control.
*
* @param currentAngle The current angle. This angle should be measured from the horizontal (i.e.
* if the provided angle is 0, the arm should be parallel to the floor). If your encoder does
* not follow this convention, an offset should be added.
* @param currentVelocity The current velocity setpoint.
* @param nextVelocity The next velocity setpoint.
* @param currentAngle The current angle in radians. This angle should be measured from the
* horizontal (i.e. if the provided angle is 0, the arm should be parallel to the floor). If
* your encoder does not follow this convention, an offset should be added.
* @param currentVelocity The current velocity setpoint in radians per second.
* @param nextVelocity The next velocity setpoint in radians per second.
* @return The computed feedforward in volts.
*/
public Voltage calculate(
Angle currentAngle, AngularVelocity currentVelocity, AngularVelocity nextVelocity) {
return Volts.of(
ArmFeedforwardJNI.calculate(
ks,
kv,
ka,
kg,
currentAngle.in(Radians),
currentVelocity.in(RadiansPerSecond),
nextVelocity.in(RadiansPerSecond),
m_dt));
public double calculateWithVelocities(
double currentAngle, double currentVelocity, double nextVelocity) {
return ArmFeedforwardJNI.calculate(
ks, kv, ka, kg, currentAngle, currentVelocity, nextVelocity, m_dt);
}
// Rearranging the main equation from the calculate() method yields the

40
wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
View File

@@ -4,13 +4,8 @@
package edu.wpi.first.math.controller;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.proto.ElevatorFeedforwardProto;
import edu.wpi.first.math.controller.struct.ElevatorFeedforwardStruct;
import edu.wpi.first.units.measure.LinearVelocity;
import edu.wpi.first.units.measure.Voltage;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
@@ -156,50 +151,31 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
* @param velocity The velocity setpoint.
* @return The computed feedforward.
*/
@SuppressWarnings("removal")
@Deprecated(forRemoval = true, since = "2025")
public double calculate(double velocity) {
return calculate(velocity, 0);
}
/**
* Calculates the feedforward from the gains and setpoints assuming discrete control when the
* setpoint does not change.
*
* @param currentVelocity The velocity setpoint.
* @return The computed feedforward.
*/
public Voltage calculate(LinearVelocity currentVelocity) {
return calculate(currentVelocity, currentVelocity);
}
/**
* Calculates the feedforward from the gains and setpoints assuming discrete control.
*
* <p>Note this method is inaccurate when the velocity crosses 0.
*
* @param currentVelocity The current velocity setpoint.
* @param nextVelocity The next velocity setpoint.
* @return The computed feedforward.
* @param currentVelocity The current velocity setpoint in meters per second.
* @param nextVelocity The next velocity setpoint in meters per second.
* @return The computed feedforward in volts.
*/
public Voltage calculate(LinearVelocity currentVelocity, LinearVelocity nextVelocity) {
public double calculateWithVelocities(double currentVelocity, double nextVelocity) {
// See wpimath/algorithms.md#Elevator_feedforward for derivation
if (ka == 0.0) {
return Volts.of(
ks * Math.signum(nextVelocity.in(MetersPerSecond))
+ kg
+ kv * nextVelocity.in(MetersPerSecond));
return ks * Math.signum(nextVelocity) + kg + kv * nextVelocity;
} else {
double A = -kv / ka;
double B = 1.0 / ka;
double A_d = Math.exp(A * m_dt);
double B_d = 1.0 / A * (A_d - 1.0) * B;
return Volts.of(
kg
+ ks * Math.signum(currentVelocity.magnitude())
+ 1.0
/ B_d
* (nextVelocity.in(MetersPerSecond) - A_d * currentVelocity.in(MetersPerSecond)));
return kg
+ ks * Math.signum(currentVelocity)
+ 1.0 / B_d * (nextVelocity - A_d * currentVelocity);
}
}

16
wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java
View File

@@ -4,9 +4,6 @@
package edu.wpi.first.math.controller;
import static edu.wpi.first.units.Units.Radians;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertThrows;
@@ -72,22 +69,15 @@ class ArmFeedforwardTest {
private void calculateAndSimulate(
double currentAngle, double currentVelocity, double nextVelocity, double dt) {
final double input =
m_armFF
.calculate(
Radians.of(currentAngle),
RadiansPerSecond.of(currentVelocity),
RadiansPerSecond.of(nextVelocity))
.in(Volts);
m_armFF.calculateWithVelocities(currentAngle, currentVelocity, nextVelocity);
assertEquals(nextVelocity, simulate(currentAngle, currentVelocity, input, dt).get(1, 0), 1e-12);
}
@Test
void testCalculate() {
// calculate(angle, angular velocity)
assertEquals(
0.5, m_armFF.calculate(Radians.of(Math.PI / 3), RadiansPerSecond.of(0)).in(Volts), 0.002);
assertEquals(
2.5, m_armFF.calculate(Radians.of(Math.PI / 3), RadiansPerSecond.of(1)).in(Volts), 0.002);
assertEquals(0.5, m_armFF.calculate(Math.PI / 3, 0.0), 0.002);
assertEquals(2.5, m_armFF.calculate(Math.PI / 3, 1.0), 0.002);
// calculate(currentAngle, currentVelocity, nextAngle, dt)
calculateAndSimulate(Math.PI / 3, 1.0, 1.05, 0.020);

8
wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java
View File

@@ -4,8 +4,6 @@
package edu.wpi.first.math.controller;
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.assertEquals;
import static org.junit.jupiter.api.Assertions.assertThrows;
@@ -26,8 +24,8 @@ class ElevatorFeedforwardTest {
@Test
void testCalculate() {
assertEquals(1, m_elevatorFF.calculate(MetersPerSecond.of(0)).in(Volts), 0.002);
assertEquals(4.5, m_elevatorFF.calculate(MetersPerSecond.of(2)).in(Volts), 0.002);
assertEquals(1, m_elevatorFF.calculate(0.0), 0.002);
assertEquals(4.5, m_elevatorFF.calculate(2.0), 0.002);
var A = MatBuilder.fill(Nat.N1(), Nat.N1(), -kv / ka);
var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / ka);
@@ -38,7 +36,7 @@ class ElevatorFeedforwardTest {
var nextR = VecBuilder.fill(3.0);
assertEquals(
plantInversion.calculate(r, nextR).get(0, 0) + ks + kg,
m_elevatorFF.calculate(MetersPerSecond.of(2.0), MetersPerSecond.of(3.0)).in(Volts),
m_elevatorFF.calculateWithVelocities(2.0, 3.0),
0.002);
}