[wpilib,wpimath] Don't use mutable units for return values

It only saves a single allocation and can cause confusing behavior on the caller (user) side.
PeterJohnson · Nov 8, 2024 · 5ab3d7d · 5ab3d7d
1 parent 661bae5
commit 5ab3d7d
Show file tree

Hide file tree

Showing 6 changed files with 28 additions and 108 deletions.
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotController.java
@@ -16,30 +16,12 @@
 import edu.wpi.first.hal.can.CANJNI;
 import edu.wpi.first.hal.can.CANStatus;
 import edu.wpi.first.units.measure.Current;
-import edu.wpi.first.units.measure.MutCurrent;
-import edu.wpi.first.units.measure.MutTemperature;
-import edu.wpi.first.units.measure.MutTime;
-import edu.wpi.first.units.measure.MutVoltage;
 import edu.wpi.first.units.measure.Temperature;
 import edu.wpi.first.units.measure.Time;
 import edu.wpi.first.units.measure.Voltage;
 
 /** Contains functions for roboRIO functionality. */
 public final class RobotController {
-  // Mutable measures
-  private static final MutTime m_mutFPGATime = Microseconds.mutable(0.0);
-  private static final MutVoltage m_mutBatteryVoltage = Volts.mutable(0.0);
-  private static final MutVoltage m_mutInputVoltage = Volts.mutable(0.0);
-  private static final MutCurrent m_mutInputCurrent = Amps.mutable(0.0);
-  private static final MutVoltage m_mutVoltage3V3 = Volts.mutable(0.0);
-  private static final MutCurrent m_mutCurrent3V3 = Amps.mutable(0.0);
-  private static final MutVoltage m_mutVoltage5V = Volts.mutable(0.0);
-  private static final MutCurrent m_mutCurrent5V = Amps.mutable(0.0);
-  private static final MutVoltage m_mutVoltage6V = Volts.mutable(0.0);
-  private static final MutCurrent m_mutCurrent6V = Amps.mutable(0.0);
-  private static final MutVoltage m_mutBrownoutVoltage = Volts.mutable(0.0);
-  private static final MutTemperature m_mutCPUTemp = Celsius.mutable(0.0);
-
   private RobotController() {
     throw new UnsupportedOperationException("This is a utility class!");
   }
@@ -109,8 +91,7 @@ public static long getFPGATime() {
    * @return The current time according to the FPGA in a measure.
    */
   public static Time getMeasureFPGATime() {
-    m_mutFPGATime.mut_replace(HALUtil.getFPGATime(), Microseconds);
-    return m_mutFPGATime;
+    return Microseconds.of(HALUtil.getFPGATime());
   }
 
   /**
@@ -141,8 +122,7 @@ public static double getBatteryVoltage() {
    * @return The battery voltage in a measure.
    */
   public static Voltage getMeasureBatteryVoltage() {
-    m_mutBatteryVoltage.mut_replace(PowerJNI.getVinVoltage(), Volts);
-    return m_mutBatteryVoltage;
+    return Volts.of(PowerJNI.getVinVoltage());
   }
 
   /**
@@ -207,8 +187,7 @@ public static double getInputVoltage() {
    * @return The controller input voltage value in a measure.
    */
   public static Voltage getMeasureInputVoltage() {
-    m_mutInputVoltage.mut_replace(PowerJNI.getVinVoltage(), Volts);
-    return m_mutInputVoltage;
+    return Volts.of(PowerJNI.getVinVoltage());
   }
 
   /**
@@ -226,8 +205,7 @@ public static double getInputCurrent() {
    * @return The controller input current value in a measure.
    */
   public static Current getMeasureInputCurrent() {
-    m_mutInputCurrent.mut_replace(PowerJNI.getVinCurrent(), Amps);
-    return m_mutInputCurrent;
+    return Amps.of(PowerJNI.getVinCurrent());
   }
 
   /**
@@ -245,8 +223,7 @@ public static double getVoltage3V3() {
    * @return The controller 3.3V rail voltage value in a measure.
    */
   public static Voltage getMeasureVoltage3V3() {
-    m_mutVoltage3V3.mut_replace(PowerJNI.getUserVoltage3V3(), Volts);
-    return m_mutVoltage3V3;
+    return Volts.of(PowerJNI.getUserVoltage3V3());
   }
 
   /**
@@ -264,8 +241,7 @@ public static double getCurrent3V3() {
    * @return The controller 3.3V rail output current value in a measure.
    */
   public static Current getMeasureCurrent3V3() {
-    m_mutCurrent3V3.mut_replace(PowerJNI.getUserCurrent3V3(), Amps);
-    return m_mutCurrent3V3;
+    return Amps.of(PowerJNI.getUserCurrent3V3());
   }
 
   /**
@@ -311,8 +287,7 @@ public static double getVoltage5V() {
    * @return The controller 5V rail voltage value in a measure.
    */
   public static Voltage getMeasureVoltage5V() {
-    m_mutVoltage5V.mut_replace(PowerJNI.getUserVoltage5V(), Volts);
-    return m_mutVoltage5V;
+    return Volts.of(PowerJNI.getUserVoltage5V());
   }
 
   /**
@@ -330,8 +305,7 @@ public static double getCurrent5V() {
    * @return The controller 5V rail output current value in a measure.
    */
   public static Current getMeasureCurrent5V() {
-    m_mutCurrent5V.mut_replace(PowerJNI.getUserCurrent5V(), Amps);
-    return m_mutCurrent5V;
+    return Amps.of(PowerJNI.getUserCurrent5V());
   }
 
   /**
@@ -377,8 +351,7 @@ public static double getVoltage6V() {
    * @return The controller 6V rail voltage value in a measure.
    */
   public static Voltage getMeasureVoltage6V() {
-    m_mutVoltage6V.mut_replace(PowerJNI.getUserVoltage6V(), Volts);
-    return m_mutVoltage6V;
+    return Volts.of(PowerJNI.getUserVoltage6V());
   }
 
   /**
@@ -396,8 +369,7 @@ public static double getCurrent6V() {
    * @return The controller 6V rail output current value in a measure.
    */
   public static Current getMeasureCurrent6V() {
-    m_mutCurrent6V.mut_replace(PowerJNI.getUserCurrent6V(), Amps);
-    return m_mutCurrent6V;
+    return Amps.of(PowerJNI.getUserCurrent6V());
   }
 
   /**
@@ -448,8 +420,7 @@ public static double getBrownoutVoltage() {
    * @return The brownout voltage in a measure.
    */
   public static Voltage getMeasureBrownoutVoltage() {
-    m_mutBrownoutVoltage.mut_replace(PowerJNI.getBrownoutVoltage(), Volts);
-    return m_mutBrownoutVoltage;
+    return Volts.of(PowerJNI.getBrownoutVoltage());
   }
 
   /**
@@ -489,8 +460,7 @@ public static double getCPUTemp() {
    * @return current CPU temperature in a measure.
    */
   public static Temperature getMeasureCPUTemp() {
-    m_mutCPUTemp.mut_replace(PowerJNI.getCPUTemp(), Celsius);
-    return m_mutCPUTemp;
+    return Celsius.of(PowerJNI.getCPUTemp());
   }
 
   /** State for the radio led. */

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java
@@ -17,9 +17,6 @@
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularAcceleration;
 import edu.wpi.first.units.measure.AngularVelocity;
-import edu.wpi.first.units.measure.MutAngle;
-import edu.wpi.first.units.measure.MutAngularAcceleration;
-import edu.wpi.first.units.measure.MutAngularVelocity;
 import edu.wpi.first.wpilibj.RobotController;
 
 /** Represents a simulated DC motor mechanism. */
@@ -33,16 +30,6 @@ public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
   // The moment of inertia for the DC motor mechanism.
   private final double m_jKgMetersSquared;
 
-  // The angle of the system.
-  private final MutAngle m_angle = Radians.mutable(0.0);
-
-  // The angular velocity of the system.
-  private final MutAngularVelocity m_angularVelocity = RadiansPerSecond.mutable(0.0);
-
-  // The angular acceleration of the system.
-  private final MutAngularAcceleration m_angularAcceleration =
-      RadiansPerSecondPerSecond.mutable(0.0);
-
   /**
    * Creates a simulated DC motor mechanism.
    *
@@ -160,8 +147,7 @@ public double getAngularPositionRotations() {
    * @return The DC motor's position
    */
   public Angle getAngularPosition() {
-    m_angle.mut_setMagnitude(getAngularPositionRad());
-    return m_angle;
+    return Radians.of(getAngularPositionRad());
   }
 
   /**
@@ -188,8 +174,7 @@ public double getAngularVelocityRPM() {
    * @return The DC motor's velocity
    */
   public AngularVelocity getAngularVelocity() {
-    m_angularVelocity.mut_setMagnitude(getAngularVelocityRadPerSec());
-    return m_angularVelocity;
+    return RadiansPerSecond.of(getAngularVelocityRadPerSec());
   }
 
   /**
@@ -208,8 +193,7 @@ public double getAngularAccelerationRadPerSecSq() {
    * @return The DC motor's acceleration.
    */
   public AngularAcceleration getAngularAcceleration() {
-    m_angularAcceleration.mut_setMagnitude(getAngularAccelerationRadPerSecSq());
-    return m_angularAcceleration;
+    return RadiansPerSecondPerSecond.of(getAngularAccelerationRadPerSecSq());
   }
 
   /**

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java
@@ -15,8 +15,6 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.AngularAcceleration;
 import edu.wpi.first.units.measure.AngularVelocity;
-import edu.wpi.first.units.measure.MutAngularAcceleration;
-import edu.wpi.first.units.measure.MutAngularVelocity;
 import edu.wpi.first.wpilibj.RobotController;
 
 /** Represents a simulated flywheel mechanism. */
@@ -30,12 +28,6 @@ public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
   // The moment of inertia for the flywheel mechanism.
   private final double m_jKgMetersSquared;
 
-  // The angular velocity of the system.
-  private final MutAngularVelocity m_angularVelocity = RadiansPerSecond.mutable(0);
-
-  // The angular acceleration of the system.
-  private final MutAngularAcceleration m_angularAcceleration = RadiansPerSecondPerSecond.mutable(0);
-
   /**
    * Creates a simulated flywheel mechanism.
    *
@@ -132,8 +124,7 @@ public double getAngularVelocityRPM() {
    * @return The flywheel's velocity
    */
   public AngularVelocity getAngularVelocity() {
-    m_angularVelocity.mut_setMagnitude(getAngularVelocityRadPerSec());
-    return m_angularVelocity;
+    return RadiansPerSecond.of(getAngularVelocityRadPerSec());
   }
 
   /**
@@ -152,8 +143,7 @@ public double getAngularAccelerationRadPerSecSq() {
    * @return The flywheel's acceleration.
    */
   public AngularAcceleration getAngularAcceleration() {
-    m_angularAcceleration.mut_setMagnitude(getAngularAccelerationRadPerSecSq());
-    return m_angularAcceleration;
+    return RadiansPerSecondPerSecond.of(getAngularAccelerationRadPerSecSq());
   }
 
   /**

diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
@@ -13,7 +13,6 @@
 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 double calculate(
    * @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 Voltage calculate(Angle currentAngle, AngularVelocity currentVelocity) {
    */
   public Voltage calculate(
       Angle currentAngle, AngularVelocity currentVelocity, AngularVelocity nextVelocity) {
-    output.mut_replace(
+    return Volts.of(
         ArmFeedforwardJNI.calculate(
             ks,
             kv,
@@ -236,9 +230,7 @@ public Voltage calculate(
             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

diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
@@ -10,7 +10,6 @@
 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 Voltage calculate(LinearVelocity currentVelocity) {
   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)));
     }
   }
 

diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
@@ -12,7 +12,6 @@
 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 <U extends Unit> Voltage calculate(
       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()));
     }
   }