diff --git a/src/main/java/xbot/common/controls/actuators/XCANMotorController.java b/src/main/java/xbot/common/controls/actuators/XCANMotorController.java index 35570c0a..1d0516a1 100644 --- a/src/main/java/xbot/common/controls/actuators/XCANMotorController.java +++ b/src/main/java/xbot/common/controls/actuators/XCANMotorController.java @@ -329,11 +329,11 @@ public void periodic() { return; } - if (softwareForwardLimit.getAsBoolean() && getVoltage().gt(Volts.of(0))) { + if (softwareForwardLimit.getAsBoolean() && getVoltage().gt(Volts.zero())) { //log.warn("Forward software limit hit"); setPower(0); } - if (softwareReverseLimit.getAsBoolean() && getVoltage().lt(Volts.of(0))) { + if (softwareReverseLimit.getAsBoolean() && getVoltage().lt(Volts.zero())) { //log.warn("Reverse software limit hit"); setPower(0); } @@ -698,12 +698,12 @@ public void refreshDataFrame() { } protected boolean isValidVoltageRequest(Voltage voltage) { - if (voltage.gt(Volts.of(0)) && softwareForwardLimit.getAsBoolean()) { + if (voltage.gt(Volts.zero()) && softwareForwardLimit.getAsBoolean()) { // TODO: Change these various warnings to only trigger once on the rising edge of the issue. //log.warn("Attempted to set positive voltage on motor controller with forward software limit enabled"); return false; } - if (voltage.lt(Volts.of(0)) && softwareReverseLimit.getAsBoolean()) { + if (voltage.lt(Volts.zero()) && softwareReverseLimit.getAsBoolean()) { //log.warn("Attempted to set negative voltage on motor controller with reverse software limit enabled"); return false; } diff --git a/src/main/java/xbot/common/controls/actuators/mock_adapters/MockCANMotorController.java b/src/main/java/xbot/common/controls/actuators/mock_adapters/MockCANMotorController.java index 3af004ec..b55dc483 100644 --- a/src/main/java/xbot/common/controls/actuators/mock_adapters/MockCANMotorController.java +++ b/src/main/java/xbot/common/controls/actuators/mock_adapters/MockCANMotorController.java @@ -8,11 +8,8 @@ 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.Current; import edu.wpi.first.units.measure.Frequency; -import edu.wpi.first.units.measure.MutAngle; -import edu.wpi.first.units.measure.MutAngularVelocity; -import edu.wpi.first.units.measure.MutCurrent; -import edu.wpi.first.units.measure.MutVoltage; import edu.wpi.first.units.measure.Time; import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.units.measure.Voltage; @@ -46,12 +43,12 @@ public enum ControlMode { private ControlMode controlMode = ControlMode.DutyCycle; private double power = 0.0; - private final MutVoltage voltage = Volts.mutable(0); - private final MutCurrent current = Amps.mutable(0); - private final MutAngle position = Rotations.mutable(0); - private final MutAngle targetPosition = Rotations.mutable(0); - private final MutAngularVelocity targetVelocity = RPM.mutable(0); - private final MutAngularVelocity velocity = RPM.mutable(0); + private Voltage voltage = Volts.zero(); + private Current current = Amps.zero(); + private Angle position = Rotations.zero(); + private Angle targetPosition = Rotations.zero(); + private AngularVelocity targetVelocity = RPM.zero(); + private AngularVelocity velocity = RPM.zero(); public double p; public double i; public double d; @@ -136,8 +133,8 @@ public void setPower(double power) { } controlMode = ControlMode.DutyCycle; this.power = MathUtil.clamp(power, -1.0, 1.0); - this.voltage.mut_replace(MathUtil.clamp(power * 12.0, -12.0, 12.0), Volts); - this.current.mut_replace(MathUtil.clamp(power, -1.0, 1.0), Amps); + this.voltage = Volts.of(MathUtil.clamp(power * 12.0, -12.0, 12.0)); + this.current = Amps.of(MathUtil.clamp(power, -1.0, 1.0)); } /* @@ -165,13 +162,13 @@ public Angle getRawPosition_internal() { @Override public void setRawPosition(Angle position) { - this.position.mut_replace(position); + this.position = position; } @Override public void setRawPositionTarget(Angle rawPosition, MotorPidMode mode, int slot) { controlMode = ControlMode.Position; - this.targetPosition.mut_replace(rawPosition); + this.targetPosition = rawPosition; } public Angle getTargetPosition() { @@ -187,17 +184,17 @@ public AngularVelocity getRawVelocity_internal() { } public void setVelocity(AngularVelocity velocity) { - this.velocity.mut_replace(convertScaledVelocityToRawVelocity(velocity)); + this.velocity = convertScaledVelocityToRawVelocity(velocity); } public void setRawVelocity(AngularVelocity rawVelocity) { - this.velocity.mut_replace(rawVelocity); + this.velocity = rawVelocity; } @Override public void setRawVelocityTarget(AngularVelocity rawVelocity, MotorPidMode mode, int slot) { controlMode = ControlMode.Velocity; - this.targetVelocity.mut_replace(rawVelocity); + this.targetVelocity = rawVelocity; } @Override @@ -214,9 +211,9 @@ public void setVoltage(Voltage voltage) { if (!isValidVoltageRequest(voltage)) { return; } - this.voltage.mut_replace(voltage); + this.voltage = voltage; this.power = MathUtil.clamp(voltage.in(Volts) / 12.0, -1.0, 1.0); - this.current.mut_replace(voltage.in(Volts) / 12.0, Amps); + this.current = Amps.of(voltage.in(Volts) / 12.0); } @Override diff --git a/src/main/java/xbot/common/controls/sensors/mock_adapters/MockCANCoder.java b/src/main/java/xbot/common/controls/sensors/mock_adapters/MockCANCoder.java index b6708e01..f4d4da43 100644 --- a/src/main/java/xbot/common/controls/sensors/mock_adapters/MockCANCoder.java +++ b/src/main/java/xbot/common/controls/sensors/mock_adapters/MockCANCoder.java @@ -4,8 +4,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.MutAngle; -import edu.wpi.first.units.measure.MutAngularVelocity; import org.json.JSONObject; import dagger.assisted.Assisted; @@ -35,8 +33,8 @@ public class MockCANCoder extends XCANCoder implements ISimulatableSensor { private double positionOffset; private final boolean inverted; - private final MutAngularVelocity velocity; - private final MutAngle position; + private AngularVelocity velocity; + private Angle position; @AssistedFactory public abstract static class MockCANCoderFactory implements XCANCoderFactory { @@ -53,8 +51,8 @@ public MockCANCoder(@Assisted("deviceInfo") DeviceInfo deviceInfo, pf.setPrefix(owningSystemPrefix); this.deviceId = deviceInfo.channel; - this.velocity = RPM.mutable(0); - this.position = Rotations.mutable(0); + this.velocity = RPM.zero(); + this.position = Rotations.zero(); pf.setDefaultLevel(Property.PropertyLevel.Debug); this.positionOffset = 0; this.inverted = deviceInfo.inverted; @@ -77,7 +75,7 @@ public Angle getAbsolutePosition_internal() { } public void setVelocity(AngularVelocity newVelocity) { - this.velocity.mut_replace(newVelocity.times(inverted ? -1 : 1)); + this.velocity = newVelocity.times(inverted ? -1 : 1); } public AngularVelocity getVelocity_internal() { @@ -86,7 +84,7 @@ public AngularVelocity getVelocity_internal() { @Override public void setPosition(Angle newPosition) { - position.mut_replace(newPosition.times(inverted ? -1 : 1)); + position = newPosition.times(inverted ? -1 : 1); } public double getPositionOffset() { @@ -94,7 +92,7 @@ public double getPositionOffset() { } public void setAbsolutePosition(Angle position) { - this.position.mut_replace(position.times(inverted ? -1 : 1)); + this.position = position.times(inverted ? -1 : 1); } @Override diff --git a/src/main/java/xbot/common/controls/sensors/wpi_adapters/LaserCANWpiAdapter.java b/src/main/java/xbot/common/controls/sensors/wpi_adapters/LaserCANWpiAdapter.java index d27b0d6d..3dfcb17a 100644 --- a/src/main/java/xbot/common/controls/sensors/wpi_adapters/LaserCANWpiAdapter.java +++ b/src/main/java/xbot/common/controls/sensors/wpi_adapters/LaserCANWpiAdapter.java @@ -62,7 +62,7 @@ public void updateInputs(LaserCANInputs inputs) { inputs.measurementLatency = XTimer.getFPGATimestampTime().minus(previousMeasurementTime); inputs.isMeasurementValid = true; } else { - inputs.distance = Meters.of(0); + inputs.distance = Meters.zero(); inputs.measurementLatency = Seconds.zero(); inputs.isMeasurementValid = false; } diff --git a/src/main/java/xbot/common/injection/electrical_contract/SparkMaxMotorControllerOutputConfig.java b/src/main/java/xbot/common/injection/electrical_contract/SparkMaxMotorControllerOutputConfig.java index fb1c0450..4f9d0423 100644 --- a/src/main/java/xbot/common/injection/electrical_contract/SparkMaxMotorControllerOutputConfig.java +++ b/src/main/java/xbot/common/injection/electrical_contract/SparkMaxMotorControllerOutputConfig.java @@ -47,7 +47,7 @@ public SparkMaxMotorControllerOutputConfig withSmartCurrentLimit(Current stallCu public SparkMaxMotorControllerOutputConfig withSmartCurrentLimit(Current stallCurrent) { this.sparkStallCurrentLimit = stallCurrent; this.sparkFreeCurrentLimit = stallCurrent; - this.sparkStallSpeed = RPM.of(0); + this.sparkStallSpeed = RPM.zero(); return this; } } diff --git a/src/main/java/xbot/common/properties/AngleProperty.java b/src/main/java/xbot/common/properties/AngleProperty.java index b9af7bfd..0ed235e8 100644 --- a/src/main/java/xbot/common/properties/AngleProperty.java +++ b/src/main/java/xbot/common/properties/AngleProperty.java @@ -2,14 +2,13 @@ import edu.wpi.first.units.AngleUnit; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.MutAngle; /** * This manages an Angle in the property system. * * @author Alex */ -public class AngleProperty extends MeasureProperty { +public class AngleProperty extends MeasureProperty { public AngleProperty(String prefix, String name, Angle defaultValue, XPropertyManager manager) { super(prefix, name, defaultValue, manager); } diff --git a/src/main/java/xbot/common/properties/AngularVelocityProperty.java b/src/main/java/xbot/common/properties/AngularVelocityProperty.java index 553553aa..868f4aa2 100644 --- a/src/main/java/xbot/common/properties/AngularVelocityProperty.java +++ b/src/main/java/xbot/common/properties/AngularVelocityProperty.java @@ -2,12 +2,11 @@ import edu.wpi.first.units.AngularVelocityUnit; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.MutAngularVelocity; /** * This manages an AngleVelocity in the property system. */ -public class AngularVelocityProperty extends MeasureProperty { +public class AngularVelocityProperty extends MeasureProperty { public AngularVelocityProperty(String prefix, String name, AngularVelocity defaultValue, XPropertyManager manager) { super(prefix, name, defaultValue, manager); } diff --git a/src/main/java/xbot/common/properties/DistanceProperty.java b/src/main/java/xbot/common/properties/DistanceProperty.java index 5a83c068..3b2392e2 100644 --- a/src/main/java/xbot/common/properties/DistanceProperty.java +++ b/src/main/java/xbot/common/properties/DistanceProperty.java @@ -2,14 +2,13 @@ import edu.wpi.first.units.DistanceUnit; import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.MutDistance; /** * This manages a Distance in the property system. * * @author Alex */ -public class DistanceProperty extends MeasureProperty { +public class DistanceProperty extends MeasureProperty { public DistanceProperty(String prefix, String name, Distance defaultValue, XPropertyManager manager) { super(prefix, name, defaultValue, manager); } diff --git a/src/main/java/xbot/common/properties/MeasureProperty.java b/src/main/java/xbot/common/properties/MeasureProperty.java index 4cbfaebe..605d819d 100644 --- a/src/main/java/xbot/common/properties/MeasureProperty.java +++ b/src/main/java/xbot/common/properties/MeasureProperty.java @@ -1,7 +1,6 @@ package xbot.common.properties; import edu.wpi.first.units.Measure; -import edu.wpi.first.units.MutableMeasure; import edu.wpi.first.units.Unit; import org.littletonrobotics.junction.LogTable; import org.littletonrobotics.junction.Logger; @@ -12,13 +11,12 @@ public class MeasureProperty< MeasureT extends Measure, - MutMeasureT extends MutableMeasure, UnitT extends Unit > extends Property { final MeasureT defaultValue; final UnitT defaultUnit; - final MutMeasureT lastValue; - final MutMeasureT currentValue; + MeasureT lastValue; + MeasureT currentValue; private final LoggableInputs inputs = new LoggableInputs() { public void toLog(LogTable table) { @@ -26,7 +24,7 @@ public void toLog(LogTable table) { } public void fromLog(LogTable table) { - currentValue.mut_replace(table.get(suffix, defaultValue)); + currentValue = table.get(suffix, defaultValue); } }; @@ -39,8 +37,8 @@ public MeasureProperty(String prefix, String name, MeasureT defaultValue, XPrope this.defaultValue = defaultValue; this.defaultUnit = defaultValue.unit(); - currentValue = (MutMeasureT) defaultValue.mutableCopy(); - lastValue = (MutMeasureT) defaultValue.mutableCopy(); + currentValue = defaultValue; + lastValue = defaultValue; // Check for non-default on load; also store a "last value" we can use @@ -49,12 +47,12 @@ public MeasureProperty(String prefix, String name, MeasureT defaultValue, XPrope if (!firstValue.isEquivalent(defaultValue)) { log.info("Property " + key + " has the non-default value " + firstValue); } - lastValue.mut_replace(firstValue); - currentValue.mut_replace((MeasureT) firstValue.copy()); + lastValue = firstValue; + currentValue = firstValue; } public MeasureT get() { - return currentValue.copy(); + return currentValue; } @@ -73,7 +71,7 @@ public MeasureT get_internal() { public void set(MeasureT value) { activeStore.setDouble(key, value.in(defaultUnit)); - currentValue.mut_replace(value); + currentValue = value; } public void hasChangedSinceLastCheck(Consumer callback) { @@ -81,23 +79,23 @@ public void hasChangedSinceLastCheck(Consumer callback) { if (!currentValue.isEquivalent(lastValue)) { callback.accept(currentValue); } - lastValue.mut_replace(currentValue); + lastValue = currentValue; } public boolean hasChangedSinceLastCheck() { MeasureT currentValue = get(); boolean changed = !currentValue.isEquivalent(lastValue); - lastValue.mut_replace(currentValue); + lastValue = currentValue; return changed; } public boolean isSetToDefault() { - return get() == defaultValue; + return get().isEquivalent(defaultValue); } @Override public void refreshDataFrame() { - currentValue.mut_replace(get_internal()); + currentValue = get_internal(); Logger.processInputs(akitLogPrefix(), inputs); } } diff --git a/src/main/java/xbot/common/properties/TimeProperty.java b/src/main/java/xbot/common/properties/TimeProperty.java index ddccd734..afcecb06 100644 --- a/src/main/java/xbot/common/properties/TimeProperty.java +++ b/src/main/java/xbot/common/properties/TimeProperty.java @@ -1,7 +1,6 @@ package xbot.common.properties; import edu.wpi.first.units.TimeUnit; -import edu.wpi.first.units.measure.MutTime; import edu.wpi.first.units.measure.Time; /** @@ -9,7 +8,7 @@ * * @author Alex */ -public class TimeProperty extends MeasureProperty { +public class TimeProperty extends MeasureProperty { public TimeProperty(String prefix, String name, Time defaultValue, XPropertyManager manager) { super(prefix, name, defaultValue, manager); } diff --git a/src/main/java/xbot/common/subsystems/pose/BasePoseSubsystem.java b/src/main/java/xbot/common/subsystems/pose/BasePoseSubsystem.java index eb914d1d..eab175d7 100644 --- a/src/main/java/xbot/common/subsystems/pose/BasePoseSubsystem.java +++ b/src/main/java/xbot/common/subsystems/pose/BasePoseSubsystem.java @@ -2,8 +2,8 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.MutAngle; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Pose2d; @@ -56,7 +56,7 @@ public abstract class BasePoseSubsystem extends BaseSubsystem implements ISwerve public static Distance fieldXMidpoint = Meters.of(8.7785); public static Distance fieldYHeight = Inches.of(317); - private final MutAngle currentHeading; + private Angle currentHeading; public BasePoseSubsystem(XGyroFactory gyroFactory, PropertyFactory propManager) { this(gyroFactory.create(), propManager); @@ -70,7 +70,7 @@ public BasePoseSubsystem(XGyro gyro, PropertyFactory propManager) { // Right when the system is initialized, we need to have the old value be // the same as the current value, to avoid any sudden changes later - currentHeading = Degrees.mutable(0); + currentHeading = Degrees.zero(); propManager.setDefaultLevel(Property.PropertyLevel.Debug); rioRotated = propManager.createPersistentProperty("RIO rotated", false); @@ -83,7 +83,7 @@ protected double getCompassHeading(Rotation2d standardHeading) { } protected void updateCurrentHeading() { - currentHeading.mut_replace(MathUtil.inputModulus(getRobotYaw().getDegrees() + headingOffset, -180, 180), Degrees); + currentHeading = Degrees.of(MathUtil.inputModulus(getRobotYaw().getDegrees() + headingOffset, -180, 180)); aKitLog.record("AdjustedHeadingDegrees", currentHeading.in(Degrees)); aKitLog.record("AdjustedHeadingRadians", currentHeading.in(Radians));