diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotController.java index 44b15609512..5e9d88d066d 100644 --- 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 index 4440a2d7787..99b546923b9 100644 --- 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 { // 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 index f1fe29daf63..95d01e16217 100644 --- 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 { // 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 index 61eb2c5932c..2f9ffe6ce72 100644 --- 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 index 8445e509c28..44212700881 100644 --- 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 index e227a63f5e6..9356481ffa0 100644 --- 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 Voltage calculate( Measure> 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())); } }