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

[wpilib,wpimath] Don't use mutable units for return values (#7369)

Browse Source 
It only saves a single allocation and can cause confusing behavior on the
caller (user) side.
This commit is contained in:
Peter Johnson
2024-11-08 19:29:51 -07:00
committed by GitHub
parent 3cc541f261
commit f40bd3593d
6 changed files with 28 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -13,7 +13,6 @@ 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.MutVoltage;
import edu.wpi.first.units.measure.Voltage;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
@@ -38,9 +37,6 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
/** The period, in seconds. */
private final double m_dt;
/** The calculated output voltage measure. */
private final MutVoltage output = Volts.mutable(0.0);
/**
* Creates a new ArmFeedforward with the specified gains and period.
*
@@ -207,12 +203,10 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
* @return The computed feedforward in volts.
*/
public Voltage calculate(Angle currentAngle, AngularVelocity currentVelocity) {
output.mut_replace(
return Volts.of(
kg * Math.cos(currentAngle.in(Radians))
+ ks * Math.signum(currentVelocity.in(RadiansPerSecond))
+ kv * currentVelocity.in(RadiansPerSecond),
Volts);
return output;
+ kv * currentVelocity.in(RadiansPerSecond));
}
/**
@@ -227,7 +221,7 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
*/
public Voltage calculate(
Angle currentAngle, AngularVelocity currentVelocity, AngularVelocity nextVelocity) {
output.mut_replace(
return Volts.of(
ArmFeedforwardJNI.calculate(
ks,
kv,
@@ -236,9 +230,7 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
currentAngle.in(Radians),
currentVelocity.in(RadiansPerSecond),
nextVelocity.in(RadiansPerSecond),
m_dt),
Volts);
return output;
m_dt));
}
// Rearranging the main equation from the calculate() method yields the

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

@@ -10,7 +10,6 @@ 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.MutVoltage;
import edu.wpi.first.units.measure.Voltage;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
@@ -35,9 +34,6 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
/** The period, in seconds. */
private final double m_dt;
/** The calculated output voltage measure. */
private final MutVoltage output = Volts.mutable(0.0);
/**
* Creates a new ElevatorFeedforward with the specified gains and period.
*
@@ -189,25 +185,21 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
public Voltage calculate(LinearVelocity currentVelocity, LinearVelocity nextVelocity) {
// See wpimath/algorithms.md#Elevator_feedforward for derivation
if (ka == 0.0) {
output.mut_replace(
return Volts.of(
ks * Math.signum(nextVelocity.in(MetersPerSecond))
+ kg
+ kv * nextVelocity.in(MetersPerSecond),
Volts);
return output;
+ kv * nextVelocity.in(MetersPerSecond));
} 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;
output.mut_replace(
return Volts.of(
kg
+ ks * Math.signum(currentVelocity.magnitude())
+ 1.0
/ B_d
* (nextVelocity.in(MetersPerSecond) - A_d * currentVelocity.in(MetersPerSecond)),
Volts);
return output;
* (nextVelocity.in(MetersPerSecond) - A_d * currentVelocity.in(MetersPerSecond)));
}
}

14
wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
View File

@@ -12,7 +12,6 @@ import edu.wpi.first.units.Measure;
import edu.wpi.first.units.PerUnit;
import edu.wpi.first.units.TimeUnit;
import edu.wpi.first.units.Unit;
import edu.wpi.first.units.measure.MutVoltage;
import edu.wpi.first.units.measure.Voltage;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
@@ -31,9 +30,6 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
/** The period, in seconds. */
private final double m_dt;
// ** The calculated output voltage measure */
private final MutVoltage output = Volts.mutable(0.0);
/**
* Creates a new SimpleMotorFeedforward with the specified gains and period.
*
@@ -189,19 +185,15 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
Measure<? extends PerUnit<U, TimeUnit>> nextVelocity) {
// See wpimath/algorithms.md#Simple_motor_feedforward for derivation
if (ka == 0.0) {
output.mut_replace(
ks * Math.signum(nextVelocity.magnitude()) + kv * nextVelocity.magnitude(), Volts);
return output;
return Volts.of(ks * Math.signum(nextVelocity.magnitude()) + kv * nextVelocity.magnitude());
} 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;
output.mut_replace(
return Volts.of(
ks * Math.signum(currentVelocity.magnitude())
+ 1.0 / B_d * (nextVelocity.magnitude() - A_d * currentVelocity.magnitude()),
Volts);
return output;
+ 1.0 / B_d * (nextVelocity.magnitude() - A_d * currentVelocity.magnitude()));
}
}