diff --git a/src/main/java/com/team766/config/AbstractConfigValue.java b/src/main/java/com/team766/config/AbstractConfigValue.java index 2d02351d4..c34179d13 100644 --- a/src/main/java/com/team766/config/AbstractConfigValue.java +++ b/src/main/java/com/team766/config/AbstractConfigValue.java @@ -1,95 +1,98 @@ package com.team766.config; -import java.util.ArrayList; -import java.util.Collection; -import java.util.Collections; +import com.team766.library.AbstractObservable; import com.team766.library.SettableValueProvider; import com.team766.logging.Category; import com.team766.logging.Logger; import com.team766.logging.LoggerExceptionUtils; import com.team766.logging.Severity; +import java.util.ArrayList; +import java.util.Collection; +import java.util.Collections; +import java.util.Optional; -public abstract class AbstractConfigValue implements SettableValueProvider { - protected String m_key; - private E m_cachedValue; - private boolean m_cachedHasValue; - private int m_cachedGeneration = -1; +public abstract class AbstractConfigValue extends AbstractObservable> + implements SettableValueProvider { + protected String m_key; + private E m_cachedValue; + private boolean m_cachedHasValue; - private static ArrayList> c_accessedValues = new ArrayList>(); + private static ArrayList> c_accessedValues = + new ArrayList>(); - static Collection> accessedValues() { - return Collections.unmodifiableCollection(c_accessedValues); - } + static Collection> accessedValues() { + return Collections.unmodifiableCollection(c_accessedValues); + } - static void resetStatics() { - c_accessedValues.clear(); - } + static void resetStatics() { + c_accessedValues.clear(); + } - protected AbstractConfigValue(final String key) { - m_key = key; - c_accessedValues.add(this); - // Querying for this config setting's key will add a placeholder entry - // in the config file if this setting does not already exist there. - ConfigFileReader.instance.getRawValue(m_key); - } + protected AbstractConfigValue(final String key) { + m_key = key; + c_accessedValues.add(this); + // Querying for this config setting's key will add a placeholder entry + // in the config file if this setting does not already exist there. + ConfigFileReader.instance.getRawValue(m_key); + update(); + } - private void sync() { - if (ConfigFileReader.instance.getGeneration() != m_cachedGeneration) { - m_cachedGeneration = ConfigFileReader.instance.getGeneration(); - var rawValue = ConfigFileReader.instance.getRawValue(m_key); - m_cachedHasValue = rawValue != null; - if (m_cachedHasValue) { - try { - m_cachedValue = parseJsonValue(rawValue); - } catch (Exception ex) { - Logger.get(Category.CONFIGURATION).logRaw(Severity.ERROR, - "Failed to parse " + m_key + " from the config file: " - + LoggerExceptionUtils.exceptionToString(ex)); - m_cachedValue = null; - m_cachedHasValue = false; - } - } - } - } + void update() { + var rawValue = ConfigFileReader.instance.getRawValue(m_key); + m_cachedHasValue = rawValue != null; + if (m_cachedHasValue) { + try { + m_cachedValue = parseJsonValue(rawValue); + } catch (Exception ex) { + Logger.get(Category.CONFIGURATION) + .logRaw( + Severity.ERROR, + "Failed to parse " + + m_key + + " from the config file: " + + LoggerExceptionUtils.exceptionToString(ex)); + m_cachedValue = null; + m_cachedHasValue = false; + } + } + notifyObservers(m_cachedHasValue ? Optional.of(m_cachedValue) : Optional.empty()); + } - public String getKey() { - return m_key; - } + public String getKey() { + return m_key; + } - @Override - public boolean hasValue() { - sync(); - return m_cachedHasValue; - } + @Override + public boolean hasValue() { + return m_cachedHasValue; + } - @Override - public E get() { - sync(); - if (!m_cachedHasValue) { - throw new IllegalArgumentException(m_key + " not found in the config file"); - } - return m_cachedValue; - } + @Override + public E get() { + if (!m_cachedHasValue) { + throw new IllegalArgumentException(m_key + " not found in the config file"); + } + return m_cachedValue; + } - public void set(final E value) { - ConfigFileReader.instance.setValue(m_key, value); - } + public void set(final E value) { + ConfigFileReader.instance.setValue(m_key, value); + } - public void clear() { - ConfigFileReader.instance.setValue(m_key, null); - } + public void clear() { + ConfigFileReader.instance.setValue(m_key, null); + } - protected abstract E parseJsonValue(Object configValue); + protected abstract E parseJsonValue(Object configValue); - @Override - public String toString() { - sync(); - if (!m_cachedHasValue) { - return ""; - } - if (m_cachedValue == null) { - return ""; - } - return m_cachedValue.toString(); - } -} \ No newline at end of file + @Override + public String toString() { + if (!m_cachedHasValue) { + return ""; + } + if (m_cachedValue == null) { + return ""; + } + return m_cachedValue.toString(); + } +} diff --git a/src/main/java/com/team766/config/ConfigFileReader.java b/src/main/java/com/team766/config/ConfigFileReader.java index 13612e36c..30f620310 100755 --- a/src/main/java/com/team766/config/ConfigFileReader.java +++ b/src/main/java/com/team766/config/ConfigFileReader.java @@ -10,6 +10,7 @@ import java.io.StringReader; import java.nio.file.Files; import java.nio.file.Paths; +import java.util.Arrays; import java.util.regex.Pattern; import org.json.JSONObject; import org.json.JSONTokener; @@ -29,10 +30,6 @@ public class ConfigFileReader { private static final String KEY_DELIMITER = "."; - // This is incremented each time the config file is reloaded to ensure that ConfigValues use the - // most recent setting. - private int m_generation = 0; - private String m_fileName; private String m_backupFileName; // if set, will also save here private JSONObject m_values = new JSONObject(); @@ -81,12 +78,11 @@ public void reloadFromJson(final String jsonString) { "Could not parse config value for " + param.getKey(), ex); } } + // All values parsed successfully; now actually apply the new values. m_values = newValues; - ++m_generation; - } - - public int getGeneration() { - return m_generation; + for (AbstractConfigValue param : AbstractConfigValue.accessedValues()) { + param.update(); + } } public boolean containsKey(final String key) { @@ -126,6 +122,13 @@ public void setValue(final String key, final E value) { String[] keyParts = splitKey(key); JSONObject parentObj = getParent(m_values, keyParts); parentObj.putOpt(keyParts[keyParts.length - 1], value == null ? JSONObject.NULL : value); + + for (AbstractConfigValue otherValue : AbstractConfigValue.accessedValues()) { + String[] otherValueKeyParts = splitKey(otherValue.getKey()); + if (isPrefix(keyParts, otherValueKeyParts) || isPrefix(otherValueKeyParts, keyParts)) { + otherValue.update(); + } + } } Object getRawValue(final String key) { @@ -152,10 +155,17 @@ private static Object getRawValue(final JSONObject obj, final String key) { return rawValue; } - private static String[] splitKey(final String key) { + static String[] splitKey(final String key) { return key.split(Pattern.quote(KEY_DELIMITER)); } + static boolean isPrefix(final String[] a, final String[] b) { + if (a.length > b.length) { + return false; + } + return Arrays.equals(a, 0, a.length, b, 0, a.length); + } + private static JSONObject getParent(JSONObject obj, final String[] keyParts) { for (int i = 0; i < keyParts.length - 1; ++i) { JSONObject subObj; diff --git a/src/main/java/com/team766/controllers/PIDController.java b/src/main/java/com/team766/controllers/PIDController.java index f4c0778d6..05a9e24b2 100755 --- a/src/main/java/com/team766/controllers/PIDController.java +++ b/src/main/java/com/team766/controllers/PIDController.java @@ -3,7 +3,6 @@ import com.team766.config.ConfigFileReader; import com.team766.hal.RobotProvider; import com.team766.library.SetValueProvider; -import com.team766.library.SettableValueProvider; import com.team766.library.ValueProvider; import com.team766.logging.Category; import com.team766.logging.Logger; @@ -40,13 +39,13 @@ public class PIDController { private int printCounter = 0; private boolean print = false; - private final ValueProvider Kp; - private final ValueProvider Ki; - private final ValueProvider Kd; - private final ValueProvider Kff; - private final ValueProvider maxoutput_low; - private final ValueProvider maxoutput_high; - private final ValueProvider endthreshold; + private ValueProvider Kp; + private ValueProvider Ki; + private ValueProvider Kd; + private ValueProvider Kff; + private ValueProvider maxoutput_low; + private ValueProvider maxoutput_high; + private ValueProvider endthreshold; private double setpoint = Double.NaN; @@ -76,6 +75,20 @@ public static PIDController loadFromConfig(String configPrefix) { ConfigFileReader.getInstance().getDouble(configPrefix + THRESHOLD_KEY)); } + /** + * Default constructor. PID gains should be set later using mutator methods. + */ + public PIDController() { + this( + new SetValueProvider(), + new SetValueProvider(), + new SetValueProvider(), + new SetValueProvider(), + new SetValueProvider(), + new SetValueProvider(), + new SetValueProvider()); + } + /** * * @param P P constant @@ -92,14 +105,7 @@ public PIDController( final double outputmax_low, final double outputmax_high, final double threshold) { - Kp = new SetValueProvider(P); - Ki = new SetValueProvider(I); - Kd = new SetValueProvider(D); - Kff = new SetValueProvider(); - maxoutput_low = new SetValueProvider(outputmax_low); - maxoutput_high = new SetValueProvider(outputmax_high); - endthreshold = new SetValueProvider(threshold); - setTimeProvider(RobotProvider.getTimeProvider()); + this(P, I, D, 0.0, outputmax_low, outputmax_high, threshold); } public PIDController( @@ -110,8 +116,14 @@ public PIDController( final double outputmax_low, final double outputmax_high, final double threshold) { - this(P, I, D, outputmax_low, outputmax_high, threshold); - ((SetValueProvider) Kff).set(FF); + Kp = new SetValueProvider(P); + Ki = new SetValueProvider(I); + Kd = new SetValueProvider(D); + Kff = new SetValueProvider(FF); + maxoutput_low = new SetValueProvider(outputmax_low); + maxoutput_high = new SetValueProvider(outputmax_high); + endthreshold = new SetValueProvider(threshold); + setTimeProvider(RobotProvider.getTimeProvider()); } private void setTimeProvider(final TimeProviderI timeProvider_) { @@ -204,30 +216,41 @@ public void disable() { * @param D Derivative value used in the PID controller */ public void setConstants(final double P, final double I, final double D) { - ((SettableValueProvider) Kp).set(P); - ((SettableValueProvider) Ki).set(I); - ((SettableValueProvider) Kd).set(D); - needsUpdate = true; + Kp = new SetValueProvider(P); + Ki = new SetValueProvider(I); + Kd = new SetValueProvider(D); } public void setP(final double P) { - ((SettableValueProvider) Kp).set(P); - needsUpdate = true; + setP(new SetValueProvider(P)); + } + + public void setP(final ValueProvider P) { + Kp = P; } public void setI(final double I) { - ((SettableValueProvider) Ki).set(I); - needsUpdate = true; + setI(new SetValueProvider(I)); + } + + public void setI(final ValueProvider I) { + Ki = I; } public void setD(final double D) { - ((SettableValueProvider) Kd).set(D); - needsUpdate = true; + setD(new SetValueProvider(D)); + } + + public void setD(final ValueProvider D) { + Kd = D; } public void setFF(final double FF) { - ((SettableValueProvider) Kff).set(FF); - needsUpdate = true; + setFF(new SetValueProvider(FF)); + } + + public void setFF(final ValueProvider FF) { + Kff = FF; } /** @@ -347,20 +370,28 @@ public double getCurrentError() { return cur_error; } - public void setMaxoutputHigh(final Double in) { - if (in == null) { - ((SettableValueProvider) maxoutput_high).clear(); - } else { - ((SettableValueProvider) maxoutput_high).set(in); - } + public void clearMaxoutputHigh() { + maxoutput_high = new SetValueProvider(); } - public void setMaxoutputLow(final Double in) { - if (in == null) { - ((SettableValueProvider) maxoutput_low).clear(); - } else { - ((SettableValueProvider) maxoutput_low).set(in); - } + public void setMaxoutputHigh(final double in) { + maxoutput_high = new SetValueProvider(in); + } + + public void setMaxoutputHigh(final ValueProvider in) { + maxoutput_high = in; + } + + public void clearMaxoutputLow() { + maxoutput_low = new SetValueProvider(); + } + + public void setMaxoutputLow(final double in) { + maxoutput_low = new SetValueProvider(in); + } + + public void setMaxoutputLow(final ValueProvider in) { + maxoutput_low = in; } public double getSetpoint() { diff --git a/src/main/java/com/team766/hal/LocalMotorController.java b/src/main/java/com/team766/hal/LocalMotorController.java index ec9095740..8e2c52975 100644 --- a/src/main/java/com/team766/hal/LocalMotorController.java +++ b/src/main/java/com/team766/hal/LocalMotorController.java @@ -4,14 +4,17 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.team766.controllers.PIDController; import com.team766.framework.Scheduler; +import com.team766.library.ValueProvider; import com.team766.logging.Category; import com.team766.logging.Logger; import com.team766.logging.LoggerExceptionUtils; import com.team766.logging.Severity; public class LocalMotorController implements MotorController { + private BasicMotorController motor; private ControlInputReader sensor; + // TODO: add support for multiple slots private PIDController pidController; private boolean inverted = false; @@ -23,16 +26,11 @@ public class LocalMotorController implements MotorController { private MotorController leader = null; public LocalMotorController( - String configPrefix, - final BasicMotorController motor_, - final ControlInputReader sensor_) { + final BasicMotorController motor_, final ControlInputReader sensor_) { this.motor = motor_; this.sensor = sensor_; - if (!configPrefix.endsWith(".")) { - configPrefix += "."; - } - this.pidController = PIDController.loadFromConfig(configPrefix + "pid."); + this.pidController = new PIDController(); Scheduler.getInstance() .add( @@ -181,7 +179,18 @@ public double getSensorVelocity() { } @Override - public void set(final ControlMode mode, final double value) { + public void set( + final ControlMode mode, final double value, int slot, double arbitraryFeedForward) { + if (slot != 0) { + throw new UnsupportedOperationException( + "Selecting PID slots not supported on LocalMotorController"); + } + + if (arbitraryFeedForward != 0.0) { + throw new UnsupportedOperationException( + "Arbitrary feed forward not supported on LocalMotorController"); + } + if (this.controlMode != mode || this.leader != null) { pidController.reset(); this.leader = null; @@ -218,22 +227,38 @@ public void setNeutralMode(final NeutralMode neutralMode) { } @Override - public void setP(final double value) { + public void setP(final ValueProvider value, int slot) { + if (slot != 0) { + throw new UnsupportedOperationException( + "Selecting PID slot not supported on LocalMotorController"); + } pidController.setP(value); } @Override - public void setI(final double value) { + public void setI(final ValueProvider value, int slot) { + if (slot != 0) { + throw new UnsupportedOperationException( + "Selecting PID slot not supported on LocalMotorController"); + } pidController.setI(value); } @Override - public void setD(final double value) { + public void setD(final ValueProvider value, int slot) { + if (slot != 0) { + throw new UnsupportedOperationException( + "Selecting PID slot not supported on LocalMotorController"); + } pidController.setD(value); } @Override - public void setFF(final double value) { + public void setFF(final ValueProvider value, int slot) { + if (slot != 0) { + throw new UnsupportedOperationException( + "Selecting PID slot not supported on LocalMotorController"); + } pidController.setFF(value); } @@ -250,7 +275,14 @@ public void setSensorInverted(final boolean inverted_) { } @Override - public void setOutputRange(final double minOutput, final double maxOutput) { + public void setOutputRange( + final ValueProvider minOutput, + final ValueProvider maxOutput, + int slot) { + if (slot != 0) { + throw new UnsupportedOperationException( + "Selecting PID slot not supported on LocalMotorController"); + } pidController.setMaxoutputLow(minOutput); pidController.setMaxoutputHigh(maxOutput); } @@ -266,12 +298,12 @@ public void setCurrentLimit(final double ampsLimit) { public void restoreFactoryDefault() { this.motor.restoreFactoryDefault(); - this.setP(0.0); - this.setI(0.0); - this.setD(0.0); - this.setFF(0.0); - this.pidController.setMaxoutputLow(null); - this.pidController.setMaxoutputHigh(null); + this.setP(0.0, 0); + this.setI(0.0, 0); + this.setD(0.0, 0); + this.setFF(0.0, 0); + this.pidController.clearMaxoutputLow(); + this.pidController.clearMaxoutputHigh(); this.inverted = false; this.sensorInverted = false; diff --git a/src/main/java/com/team766/hal/MotorController.java b/src/main/java/com/team766/hal/MotorController.java index 1e3a5d8b7..a9daa42c2 100755 --- a/src/main/java/com/team766/hal/MotorController.java +++ b/src/main/java/com/team766/hal/MotorController.java @@ -2,6 +2,8 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.team766.library.SetValueProvider; +import com.team766.library.ValueProvider; /** * Interface for motor controlling devices. @@ -25,7 +27,7 @@ enum ControlMode { } /** - * Common interface for setting the power outputu by a motor controller. + * Common interface for setting the power output by a motor controller. * * @param power The power to set. Value should be between -1.0 and 1.0. */ @@ -44,7 +46,26 @@ enum ControlMode { * * @param value The setpoint value, as described above. */ - void set(ControlMode mode, double value); + default void set(ControlMode mode, double value) { + set(mode, value, 0, 0.0); + } + + /** + * Sets the appropriate output on the motor controller, depending on the mode. + * @param mode The output mode to apply. + * In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped. + * In Current mode, output value is in amperes. + * In Velocity mode, output value is in position change / 100ms. + * In Position mode, output value is in encoder ticks or an analog value, + * depending on the sensor. + * In Follower mode, the output value is the integer device ID of the talon to + * duplicate. + * + * @param value The setpoint value, as described above. + * @param slot The PID slot to use, if the mode uses PID. + * @param arbitraryFeedForward An arbitrary feed forward value to use when calculating PID. + */ + void set(ControlMode mode, double value, int slot, double arbitraryFeedForward); /** * Common interface for inverting direction of a motor controller. @@ -92,13 +113,33 @@ enum ControlMode { void setNeutralMode(NeutralMode neutralMode); - void setP(double value); + default int numPIDSlots() { + return 1; + } - void setI(double value); + void setP(ValueProvider value, int slot); - void setD(double value); + default void setP(double value, int slot) { + setP(new SetValueProvider(value), slot); + } - void setFF(double value); + void setI(ValueProvider value, int slot); + + default void setI(double value, int slot) { + setI(new SetValueProvider(value), slot); + } + + void setD(ValueProvider value, int slot); + + default void setD(double value, int slot) { + setD(new SetValueProvider(value), slot); + } + + void setFF(ValueProvider value, int slot); + + default void setFF(double value, int slot) { + setFF(new SetValueProvider(value), slot); + } void setSelectedFeedbackSensor(FeedbackDevice feedbackDevice); @@ -112,7 +153,14 @@ enum ControlMode { */ void setSensorInverted(boolean inverted); - void setOutputRange(double minOutput, double maxOutput); + void setOutputRange(ValueProvider minOutput, ValueProvider maxOutput, int slot); + + default void setOutputRange(double minOutput, double maxOutput, int slot) { + setOutputRange( + new SetValueProvider(minOutput), + new SetValueProvider(maxOutput), + slot); + } void setCurrentLimit(double ampsLimit); diff --git a/src/main/java/com/team766/hal/MotorControllerWithSensorScale.java b/src/main/java/com/team766/hal/MotorControllerWithSensorScale.java index 59f30c5cc..eaf281c6b 100644 --- a/src/main/java/com/team766/hal/MotorControllerWithSensorScale.java +++ b/src/main/java/com/team766/hal/MotorControllerWithSensorScale.java @@ -2,6 +2,8 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.team766.library.TransformingValueProvider; +import com.team766.library.ValueProvider; public class MotorControllerWithSensorScale implements MotorController { private MotorController delegate; @@ -12,6 +14,11 @@ public MotorControllerWithSensorScale(final MotorController delegate_, final dou this.scale = scale_; } + @Override + public int numPIDSlots() { + return delegate.numPIDSlots(); + } + @Override public double getSensorPosition() { return delegate.getSensorPosition() * scale; @@ -23,16 +30,17 @@ public double getSensorVelocity() { } @Override - public void set(final ControlMode mode, final double value) { + public void set( + final ControlMode mode, final double value, int slot, double arbitraryFeedForward) { switch (mode) { case PercentOutput: delegate.set(mode, value); return; case Position: - delegate.set(mode, value / scale); + delegate.set(mode, value / scale, slot, arbitraryFeedForward); return; case Velocity: - delegate.set(mode, value / scale); + delegate.set(mode, value / scale, slot, arbitraryFeedForward); return; case Voltage: delegate.set(mode, value); @@ -78,23 +86,23 @@ public void setNeutralMode(final NeutralMode neutralMode) { } @Override - public void setP(final double value) { - delegate.setP(value * scale); + public void setP(final ValueProvider value, int slot) { + delegate.setP(new TransformingValueProvider<>(value, v -> v * scale), slot); } @Override - public void setI(final double value) { - delegate.setI(value * scale); + public void setI(final ValueProvider value, int slot) { + delegate.setI(new TransformingValueProvider<>(value, v -> v * scale), slot); } @Override - public void setD(final double value) { - delegate.setD(value * scale); + public void setD(final ValueProvider value, int slot) { + delegate.setD(new TransformingValueProvider<>(value, v -> v * scale), slot); } @Override - public void setFF(final double value) { - delegate.setFF(value * scale); + public void setFF(final ValueProvider value, int slot) { + delegate.setFF(new TransformingValueProvider<>(value, v -> v * scale), slot); } @Override @@ -108,8 +116,11 @@ public void setSensorInverted(final boolean inverted) { } @Override - public void setOutputRange(final double minOutput, final double maxOutput) { - delegate.setOutputRange(minOutput, maxOutput); + public void setOutputRange( + final ValueProvider minOutput, + final ValueProvider maxOutput, + int slot) { + delegate.setOutputRange(minOutput, maxOutput, slot); } @Override diff --git a/src/main/java/com/team766/hal/PIDSlotHelper.java b/src/main/java/com/team766/hal/PIDSlotHelper.java new file mode 100644 index 000000000..97ef98ece --- /dev/null +++ b/src/main/java/com/team766/hal/PIDSlotHelper.java @@ -0,0 +1,121 @@ +package com.team766.hal; + +import com.team766.library.ObserveValue; +import com.team766.library.ValueProvider; +import edu.wpi.first.math.Pair; + +public class PIDSlotHelper { + private class Slot { + private final int slot; + final ObserveValue pGain; + final ObserveValue iGain; + final ObserveValue dGain; + final ObserveValue ffGain; + final ObserveValue outputMin; + final ObserveValue outputMax; + + private Slot(int slot) { + this.slot = slot; + this.pGain = + new ObserveValue( + ObserveValue.whenPresent((p) -> motor.setP_Impl(p, this.slot))); + this.iGain = + new ObserveValue( + ObserveValue.whenPresent((i) -> motor.setI_Impl(i, this.slot))); + this.dGain = + new ObserveValue( + ObserveValue.whenPresent((d) -> motor.setD_Impl(d, this.slot))); + this.ffGain = + new ObserveValue( + ObserveValue.whenPresent((ff) -> motor.setFF_Impl(ff, this.slot))); + this.outputMin = + new ObserveValue(ObserveValue.whenPresent((__) -> updateOutputRange())); + this.outputMax = + new ObserveValue(ObserveValue.whenPresent((__) -> updateOutputRange())); + } + + private void updateOutputRange() { + motor.setOutputRange_Impl( + outputMin.getValueProvider().valueOr(-1.0), + outputMax.getValueProvider().valueOr(1.0), + slot); + } + } + + /** + * The methods of this interface match the semantics of the similarly-named methods in + * {@link MotorController}, but it's important that these set_Impl methods remain separate from + * the MotorController methods, otherwise PIDSlotHelper will not work properly. + * All of the MotorController methods should call PIDSlotHelper (rather than updating the motor + * device object directly) otherwise PIDSlotHelper's ObserveValues will not be updated, and they + * will stay subscribed to the old ValueProvider. + */ + public static interface MotorCallbacks { + int numPIDSlots(); + + void setP_Impl(double value, int slot); + + void setI_Impl(double value, int slot); + + void setD_Impl(double value, int slot); + + void setFF_Impl(double value, int slot); + + void setOutputRange_Impl(double minOutput, double maxOutput, int slot); + } + + private final MotorCallbacks motor; + private final Slot[] slots; + + public PIDSlotHelper(MotorCallbacks motor) { + this.motor = motor; + final int size = motor.numPIDSlots(); + this.slots = new Slot[size]; + for (int i = 0; i < size; ++i) { + this.slots[i] = new Slot(i); + } + } + + public ValueProvider getP(int slot) { + return slots[slot].pGain.getValueProvider(); + } + + public void setP(ValueProvider value, int slot) { + slots[slot].pGain.setValueProvider(value); + } + + public ValueProvider getI(int slot) { + return slots[slot].iGain.getValueProvider(); + } + + public void setI(ValueProvider value, int slot) { + slots[slot].iGain.setValueProvider(value); + } + + public ValueProvider getD(int slot) { + return slots[slot].dGain.getValueProvider(); + } + + public void setD(ValueProvider value, int slot) { + slots[slot].dGain.setValueProvider(value); + } + + public ValueProvider getFF(int slot) { + return slots[slot].ffGain.getValueProvider(); + } + + public void setFF(ValueProvider value, int slot) { + slots[slot].ffGain.setValueProvider(value); + } + + public Pair, ValueProvider> getOutputRange(int slot) { + return new Pair, ValueProvider>( + slots[slot].outputMin.getValueProvider(), slots[slot].outputMax.getValueProvider()); + } + + public void setOutputRange( + ValueProvider minValue, ValueProvider maxValue, int slot) { + slots[slot].outputMin.setValueProvider(minValue); + slots[slot].outputMax.setValueProvider(maxValue); + } +} diff --git a/src/main/java/com/team766/hal/RobotProvider.java b/src/main/java/com/team766/hal/RobotProvider.java index 5f8d376cc..10488e770 100755 --- a/src/main/java/com/team766/hal/RobotProvider.java +++ b/src/main/java/com/team766/hal/RobotProvider.java @@ -147,8 +147,13 @@ public MotorController getMotor(final String configName) { if (sensorInvertedConfig.valueOr(false)) { motor.setSensorInverted(true); } - // check for, apply any PID settings that are in a sub-config - configurePID(configName + ".pid.", motor); + // check for, apply any PID settings that are in slot-specific sub-configs + configurePID(configName + ".pid.", motor, 0); + // TODO: simplify this just to support 2 slots in the config, if the motor supports > 1 + // slot? + for (int slot = 1; slot < motor.numPIDSlots(); ++slot) { + configurePID(configName + ".pid" + slot + ".", motor, slot); + } return motor; } catch (IllegalArgumentException ex) { @@ -158,11 +163,11 @@ public MotorController getMotor(final String configName) { "Error getting configuration for motor %s from config file, using mock motor instead.\nDetailed error: %s", configName, LoggerExceptionUtils.exceptionToString(ex)); - return new LocalMotorController(configName, new MockMotorController(0), sensor); + return new LocalMotorController(new MockMotorController(0), sensor); } } - private void configurePID(final String configName, MotorController motor) { + private void configurePID(final String configName, MotorController motor, int slot) { ValueProvider pValue = ConfigFileReader.getInstance().getDouble(configName + PIDController.P_GAIN_KEY); ValueProvider iValue = @@ -178,25 +183,11 @@ private void configurePID(final String configName, MotorController motor) { ConfigFileReader.getInstance() .getDouble(configName + PIDController.OUTPUT_MAX_HIGH_KEY); - if (pValue.hasValue()) { - motor.setP(pValue.get()); - } - - if (iValue.hasValue()) { - motor.setI(iValue.get()); - } - - if (dValue.hasValue()) { - motor.setD(dValue.get()); - } - - if (ffValue.hasValue()) { - motor.setFF(ffValue.get()); - } - - if (outputMaxLowValue.hasValue() || outputMaxHighValue.hasValue()) { - motor.setOutputRange(outputMaxLowValue.valueOr(-1.0), outputMaxHighValue.valueOr(1.0)); - } + motor.setP(pValue, slot); + motor.setI(iValue, slot); + motor.setD(dValue, slot); + motor.setFF(ffValue, slot); + motor.setOutputRange(outputMaxLowValue, outputMaxHighValue, slot); } public EncoderReader getEncoder(final String configName) { diff --git a/src/main/java/com/team766/hal/mock/TestRobotProvider.java b/src/main/java/com/team766/hal/mock/TestRobotProvider.java index 1bac8f38d..67c629140 100755 --- a/src/main/java/com/team766/hal/mock/TestRobotProvider.java +++ b/src/main/java/com/team766/hal/mock/TestRobotProvider.java @@ -33,7 +33,6 @@ public MotorController getMotor( if (motors[index] == null) { motors[index] = new LocalMotorController( - configPrefix, new MockMotorController(index), localSensor != null ? localSensor : new MockEncoder()); } diff --git a/src/main/java/com/team766/hal/simulator/SimMotorController.java b/src/main/java/com/team766/hal/simulator/SimMotorController.java index 7f8862212..f2f9d47a2 100755 --- a/src/main/java/com/team766/hal/simulator/SimMotorController.java +++ b/src/main/java/com/team766/hal/simulator/SimMotorController.java @@ -1,57 +1,56 @@ package com.team766.hal.simulator; -import com.team766.hal.ControlInputReader; import com.team766.hal.BasicMotorController; +import com.team766.hal.ControlInputReader; import com.team766.hal.LocalMotorController; import com.team766.simulator.ProgramInterface; public class SimMotorController extends LocalMotorController { - public SimMotorController(final String configPrefix, final int address) { - this(configPrefix, ProgramInterface.canMotorControllerChannels[address]); - } - - SimMotorController(final String configPrefix, - final ProgramInterface.CANMotorControllerCommunication channel) { - super(configPrefix, new SimBasicMotorController(channel), new ControlInputReader() { - @Override - public double getPosition() { - return channel.status.sensorPosition; - } - - @Override - public double getRate() { - return channel.status.sensorVelocity; - } - }); - } + public SimMotorController(final int address) { + this(ProgramInterface.canMotorControllerChannels[address]); + } + + SimMotorController(final ProgramInterface.CANMotorControllerCommunication channel) { + super( + new SimBasicMotorController(channel), + new ControlInputReader() { + @Override + public double getPosition() { + return channel.status.sensorPosition; + } + + @Override + public double getRate() { + return channel.status.sensorVelocity; + } + }); + } } - class SimBasicMotorController implements BasicMotorController { - private final ProgramInterface.CANMotorControllerCommunication channel; - - SimBasicMotorController(final int address) { - this(ProgramInterface.canMotorControllerChannels[address]); - } - - SimBasicMotorController(final ProgramInterface.CANMotorControllerCommunication channel_) { - this.channel = channel_; - } - - @Override - public double get() { - return channel.command.output; - } - - @Override - public void set(double power) { - power = Math.min(Math.max(-1, power), 1); - channel.command.output = power; - channel.command.controlMode = - ProgramInterface.CANMotorControllerCommand.ControlMode.PercentOutput; - } - - @Override - public void restoreFactoryDefault() { - } -} \ No newline at end of file + private final ProgramInterface.CANMotorControllerCommunication channel; + + SimBasicMotorController(final int address) { + this(ProgramInterface.canMotorControllerChannels[address]); + } + + SimBasicMotorController(final ProgramInterface.CANMotorControllerCommunication channel_) { + this.channel = channel_; + } + + @Override + public double get() { + return channel.command.output; + } + + @Override + public void set(double power) { + power = Math.min(Math.max(-1, power), 1); + channel.command.output = power; + channel.command.controlMode = + ProgramInterface.CANMotorControllerCommand.ControlMode.PercentOutput; + } + + @Override + public void restoreFactoryDefault() {} +} diff --git a/src/main/java/com/team766/hal/simulator/SimulationRobotProvider.java b/src/main/java/com/team766/hal/simulator/SimulationRobotProvider.java index faa9a9669..e57bb7125 100755 --- a/src/main/java/com/team766/hal/simulator/SimulationRobotProvider.java +++ b/src/main/java/com/team766/hal/simulator/SimulationRobotProvider.java @@ -32,10 +32,9 @@ public MotorController getMotor( if (motors[index] == null) { if (localSensor != null) { motors[index] = - new LocalMotorController( - configPrefix, new SimBasicMotorController(index), localSensor); + new LocalMotorController(new SimBasicMotorController(index), localSensor); } else { - motors[index] = new SimMotorController(configPrefix, index); + motors[index] = new SimMotorController(index); } } return motors[index]; diff --git a/src/main/java/com/team766/hal/wpilib/CANSparkMaxMotorController.java b/src/main/java/com/team766/hal/wpilib/CANSparkMaxMotorController.java index 932e1fc5f..0c2078c57 100644 --- a/src/main/java/com/team766/hal/wpilib/CANSparkMaxMotorController.java +++ b/src/main/java/com/team766/hal/wpilib/CANSparkMaxMotorController.java @@ -8,17 +8,23 @@ import com.revrobotics.SparkAnalogSensor; import com.team766.hal.MotorController; import com.team766.hal.MotorControllerCommandFailedException; +import com.team766.hal.PIDSlotHelper; +import com.team766.library.ValueProvider; import com.team766.logging.LoggerExceptionUtils; import java.util.function.Function; import java.util.function.Supplier; -public class CANSparkMaxMotorController extends CANSparkMax implements MotorController { +public class CANSparkMaxMotorController extends CANSparkMax + implements MotorController, PIDSlotHelper.MotorCallbacks { + + private static final int NUM_PID_SLOTS = 2; // should be 4, only exposing 2 at this time. private Supplier sensorPositionSupplier; private Supplier sensorVelocitySupplier; private Function sensorPositionSetter; private Function sensorInvertedSetter; private boolean sensorInverted = false; + private final PIDSlotHelper pidSlotHelper; public CANSparkMaxMotorController(final int deviceId) { super(deviceId, MotorType.kBrushless); @@ -27,6 +33,7 @@ public CANSparkMaxMotorController(final int deviceId) { // getSensorPosition/getSensorVelocity return values that match what the // device's PID controller is using. setSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); + pidSlotHelper = new PIDSlotHelper(this); } private enum ExceptionTarget { @@ -60,22 +67,43 @@ public double getSensorVelocity() { } @Override - public void set(final ControlMode mode, final double value) { + public void set( + final ControlMode mode, final double value, int slot, double arbitraryFeedForward) { switch (mode) { case Disabled: disable(); break; case PercentOutput: - getPIDController().setReference(value, CANSparkMax.ControlType.kDutyCycle); + getPIDController() + .setReference( + value, + CANSparkMax.ControlType.kDutyCycle, + slot, + arbitraryFeedForward); break; case Position: - getPIDController().setReference(value, CANSparkMax.ControlType.kPosition); + getPIDController() + .setReference( + value, + CANSparkMax.ControlType.kPosition, + slot, + arbitraryFeedForward); break; case Velocity: - getPIDController().setReference(value, CANSparkMax.ControlType.kVelocity); + getPIDController() + .setReference( + value, + CANSparkMax.ControlType.kVelocity, + slot, + arbitraryFeedForward); break; case Voltage: - getPIDController().setReference(value, CANSparkMax.ControlType.kVoltage); + getPIDController() + .setReference( + value, + CANSparkMax.ControlType.kVoltage, + slot, + arbitraryFeedForward); default: throw new IllegalArgumentException("Unsupported control mode " + mode); } @@ -88,8 +116,12 @@ public void setSensorPosition(final double position) { @Override public void follow(final MotorController leader) { + // see if the follow request should specify inverting the motor. + // we do this based on whether or not the leader and follower have different inversion + // settings. + boolean invert = getInverted() != leader.getInverted(); try { - revErrorToException(ExceptionTarget.LOG, super.follow((CANSparkMax) leader)); + revErrorToException(ExceptionTarget.LOG, super.follow((CANSparkMax) leader, invert)); } catch (ClassCastException ex) { LoggerExceptionUtils.logException( new IllegalArgumentException( @@ -114,23 +146,48 @@ public void setNeutralMode(final NeutralMode neutralMode) { } @Override - public void setP(final double value) { - revErrorToException(ExceptionTarget.LOG, getPIDController().setP(value)); + public int numPIDSlots() { + return NUM_PID_SLOTS; + } + + @Override + public void setP(ValueProvider value, int slot) { + pidSlotHelper.setP(value, slot); + } + + @Override + public void setP_Impl(final double value, int slot) { + revErrorToException(ExceptionTarget.LOG, getPIDController().setP(value, slot)); + } + + @Override + public void setI(ValueProvider value, int slot) { + pidSlotHelper.setI(value, slot); } @Override - public void setI(final double value) { - revErrorToException(ExceptionTarget.LOG, getPIDController().setI(value)); + public void setI_Impl(final double value, int slot) { + revErrorToException(ExceptionTarget.LOG, getPIDController().setI(value, slot)); } @Override - public void setD(final double value) { - revErrorToException(ExceptionTarget.LOG, getPIDController().setD(value)); + public void setD(ValueProvider value, int slot) { + pidSlotHelper.setD(value, slot); } @Override - public void setFF(final double value) { - revErrorToException(ExceptionTarget.LOG, getPIDController().setFF(value)); + public void setD_Impl(final double value, int slot) { + revErrorToException(ExceptionTarget.LOG, getPIDController().setD(value, slot)); + } + + @Override + public void setFF(ValueProvider value, int slot) { + pidSlotHelper.setFF(value, slot); + } + + @Override + public void setFF_Impl(final double value, int slot) { + revErrorToException(ExceptionTarget.LOG, getPIDController().setFF(value, slot)); } @Override @@ -220,9 +277,15 @@ public void setSensorInverted(final boolean inverted) { } @Override - public void setOutputRange(final double minOutput, final double maxOutput) { + public void setOutputRange( + ValueProvider minOutput, ValueProvider maxOutput, int slot) { + pidSlotHelper.setOutputRange(minOutput, maxOutput, slot); + } + + @Override + public void setOutputRange_Impl(final double minOutput, final double maxOutput, int slot) { revErrorToException( - ExceptionTarget.LOG, getPIDController().setOutputRange(minOutput, maxOutput)); + ExceptionTarget.LOG, getPIDController().setOutputRange(minOutput, maxOutput, slot)); } public void setCurrentLimit(final double ampsLimit) { diff --git a/src/main/java/com/team766/hal/wpilib/CANTalonFxMotorController.java b/src/main/java/com/team766/hal/wpilib/CANTalonFxMotorController.java index 25f8b2833..5ac780cd6 100644 --- a/src/main/java/com/team766/hal/wpilib/CANTalonFxMotorController.java +++ b/src/main/java/com/team766/hal/wpilib/CANTalonFxMotorController.java @@ -20,20 +20,30 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.team766.hal.MotorController; import com.team766.hal.MotorControllerCommandFailedException; +import com.team766.hal.PIDSlotHelper; +import com.team766.library.ValueProvider; +import com.team766.logging.Category; +import com.team766.logging.Logger; import com.team766.logging.LoggerExceptionUtils; +import com.team766.logging.Severity; -public class CANTalonFxMotorController extends TalonFX implements MotorController { +public class CANTalonFxMotorController extends TalonFX + implements MotorController, PIDSlotHelper.MotorCallbacks { + + private static final int NUM_PID_SLOTS = 2; // NOTE: whenever we make changes to this (or embedded parts of it), we refresh the config // out of paranoia, in case some code casts this MotorController to a TalonFX directly // and changes a configuration, bypassing this code. private TalonFXConfiguration talonFXConfig = new TalonFXConfiguration(); + private final PIDSlotHelper pidSlotHelper; // TODO: add support for taking a CANcoder as a ctor parameter public CANTalonFxMotorController(final int deviceNumber, final String canBus) { super(deviceNumber, canBus); TalonFXConfigurator configurator = getConfigurator(); statusCodeToException(ExceptionTarget.LOG, configurator.refresh(talonFXConfig)); + pidSlotHelper = new PIDSlotHelper(this); } public CANTalonFxMotorController(final int deviceNumber) { @@ -66,24 +76,48 @@ private void refreshConfig() { } @Override - public void set(final ControlMode mode, double value) { + public void set( + final ControlMode mode, double value, int pidSlot, double arbitraryFeedForward) { switch (mode) { case Disabled: super.disable(); break; case PercentOutput: + // PID values don't apply here. + // TODO: soften this to a warning? + if ((pidSlot != 0) || (arbitraryFeedForward != 0.0)) { + throw new IllegalArgumentException( + "PercentOutput requested with PID slot (" + + pidSlot + + ") or feedForward (" + + arbitraryFeedForward + + ")"); + } DutyCycleOut percent = new DutyCycleOut(value); super.setControl(percent); break; case Position: PositionDutyCycle position = new PositionDutyCycle(value); + position.Slot = pidSlot; super.setControl(position); break; case Velocity: VelocityDutyCycle velocity = new VelocityDutyCycle(value); + velocity.Slot = pidSlot; + velocity.FeedForward = arbitraryFeedForward; super.setControl(velocity); break; case Voltage: + // PID values don't apply here. + // TODO: soften this to a warning? + if ((pidSlot != 0) || (arbitraryFeedForward != 0.0)) { + throw new IllegalArgumentException( + "PercentOutput requested with PID slot (" + + pidSlot + + ") or feedForward (" + + arbitraryFeedForward + + ")"); + } VoltageOut voltage = new VoltageOut(value); super.setControl(voltage); break; @@ -151,33 +185,92 @@ public void setClosedLoopRamp(final double secondsFromNeutralToFull) { } @Override - public void setFF(final double value) { + public int numPIDSlots() { + return NUM_PID_SLOTS; + } + + @Override + public void setFF(ValueProvider value, int slot) { + pidSlotHelper.setFF(value, slot); + } + + @Override + public void setFF_Impl(final double value, int slot) { refreshConfig(); - talonFXConfig.Slot0.kV = value; - statusCodeToException(ExceptionTarget.LOG, getConfigurator().apply(talonFXConfig.Slot0)); + switch (slot) { + case 0: + talonFXConfig.Slot0.kV = value; + break; + case 1: + talonFXConfig.Slot1.kV = value; + break; + default: + throw new IllegalArgumentException("Unsupported slot " + slot); + } + statusCodeToException(ExceptionTarget.LOG, getConfigurator().apply(talonFXConfig)); + } + + @Override + public void setP(final ValueProvider value, int slot) { + pidSlotHelper.setP(value, slot); } @Override - public void setP(final double value) { + public void setP_Impl(final double value, int slot) { refreshConfig(); - talonFXConfig.Slot0.kP = value; - statusCodeToException(ExceptionTarget.LOG, getConfigurator().apply(talonFXConfig.Slot0)); + switch (slot) { + case 0: + talonFXConfig.Slot0.kP = value; + break; + case 1: + talonFXConfig.Slot1.kP = value; + break; + default: + throw new IllegalArgumentException("Unsupported slot " + slot); + } + statusCodeToException(ExceptionTarget.LOG, getConfigurator().apply(talonFXConfig)); } @Override - public void setI(final double value) { + public void setI(final ValueProvider value, int slot) { + pidSlotHelper.setI(value, slot); + } + + @Override + public void setI_Impl(final double value, int slot) { refreshConfig(); - // code. - talonFXConfig.Slot0.kI = value; - statusCodeToException(ExceptionTarget.LOG, getConfigurator().apply(talonFXConfig.Slot0)); + switch (slot) { + case 0: + talonFXConfig.Slot0.kI = value; + break; + case 1: + talonFXConfig.Slot1.kI = value; + break; + default: + throw new IllegalArgumentException("Unsupported slot " + slot); + } + statusCodeToException(ExceptionTarget.LOG, getConfigurator().apply(talonFXConfig)); + } + + @Override + public void setD(final ValueProvider value, int slot) { + pidSlotHelper.setD(value, slot); } @Override - public void setD(final double value) { + public void setD_Impl(final double value, int slot) { refreshConfig(); - // code. - talonFXConfig.Slot0.kD = value; - statusCodeToException(ExceptionTarget.LOG, getConfigurator().apply(talonFXConfig.Slot0)); + switch (slot) { + case 0: + talonFXConfig.Slot0.kD = value; + break; + case 1: + talonFXConfig.Slot1.kD = value; + break; + default: + throw new IllegalArgumentException("Unsupported slot " + slot); + } + statusCodeToException(ExceptionTarget.LOG, getConfigurator().apply(talonFXConfig)); } private void setRemoteFeedbackSensor(CANcoder canCoder) { @@ -215,7 +308,19 @@ public void setSensorInverted(final boolean inverted) { } @Override - public void setOutputRange(final double minOutput, final double maxOutput) { + public void setOutputRange( + ValueProvider minOutput, ValueProvider maxOutput, int slot) { + pidSlotHelper.setOutputRange(minOutput, maxOutput, slot); + } + + @Override + public void setOutputRange_Impl(final double minOutput, final double maxOutput, int slot) { + if (slot != 0) { + Logger.get(Category.HAL) + .logRaw( + Severity.WARNING, + "Ignoring slot for setOutputRange - unsupported on TalonFX"); + } MotorOutputConfigs motorOutput = new MotorOutputConfigs(); statusCodeToException(ExceptionTarget.LOG, getConfigurator().refresh(motorOutput)); diff --git a/src/main/java/com/team766/hal/wpilib/CANTalonMotorController.java b/src/main/java/com/team766/hal/wpilib/CANTalonMotorController.java index 5d6f884a3..3072462c3 100644 --- a/src/main/java/com/team766/hal/wpilib/CANTalonMotorController.java +++ b/src/main/java/com/team766/hal/wpilib/CANTalonMotorController.java @@ -6,22 +6,27 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.team766.hal.MotorController; +import com.team766.hal.PIDSlotHelper; +import com.team766.library.ValueProvider; import com.team766.logging.Category; import com.team766.logging.Logger; import com.team766.logging.LoggerExceptionUtils; import com.team766.logging.Severity; -public class CANTalonMotorController extends BaseCTREMotorController implements MotorController { +public class CANTalonMotorController extends BaseCTREMotorController + implements MotorController, PIDSlotHelper.MotorCallbacks { + private static final int NUM_PID_SLOTS = 2; private WPI_TalonSRX m_device; - private double m_feedForward = 0.0; + private final PIDSlotHelper pidSlotHelper; public CANTalonMotorController(final int deviceNumber) { m_device = new WPI_TalonSRX(deviceNumber); + pidSlotHelper = new PIDSlotHelper(this); } @Override - public void set(final ControlMode mode, double value) { + public void set(final ControlMode mode, double value, int slot, double arbitraryFeedForward) { com.ctre.phoenix.motorcontrol.ControlMode ctre_mode = null; boolean useFourTermSet = true; switch (mode) { @@ -56,7 +61,7 @@ public void set(final ControlMode mode, double value) { ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.Disabled; } if (useFourTermSet) { - m_device.set(ctre_mode, value, DemandType.ArbitraryFeedForward, m_feedForward); + m_device.set(ctre_mode, value, DemandType.ArbitraryFeedForward, arbitraryFeedForward); } else { m_device.set(ctre_mode, value); } @@ -110,23 +115,48 @@ public void setClosedLoopRamp(final double secondsFromNeutralToFull) { } @Override - public void setFF(final double value) { - errorCodeToException(ExceptionTarget.LOG, m_device.config_kF(0, value, TIMEOUT_MS)); + public int numPIDSlots() { + return NUM_PID_SLOTS; } @Override - public void setP(final double value) { - errorCodeToException(ExceptionTarget.LOG, m_device.config_kP(0, value)); + public void setFF(ValueProvider value, int slot) { + pidSlotHelper.setFF(value, slot); } @Override - public void setI(final double value) { - errorCodeToException(ExceptionTarget.LOG, m_device.config_kI(0, value)); + public void setP(ValueProvider value, int slot) { + pidSlotHelper.setP(value, slot); } @Override - public void setD(final double value) { - errorCodeToException(ExceptionTarget.LOG, m_device.config_kD(0, value)); + public void setI(ValueProvider value, int slot) { + pidSlotHelper.setI(value, slot); + } + + @Override + public void setD(ValueProvider value, int slot) { + pidSlotHelper.setD(value, slot); + } + + @Override + public void setFF_Impl(final double value, int slot) { + errorCodeToException(ExceptionTarget.LOG, m_device.config_kF(slot, value, TIMEOUT_MS)); + } + + @Override + public void setP_Impl(final double value, int slot) { + errorCodeToException(ExceptionTarget.LOG, m_device.config_kP(slot, value)); + } + + @Override + public void setI_Impl(final double value, int slot) { + errorCodeToException(ExceptionTarget.LOG, m_device.config_kI(slot, value)); + } + + @Override + public void setD_Impl(final double value, int slot) { + errorCodeToException(ExceptionTarget.LOG, m_device.config_kD(slot, value)); } @Override @@ -141,7 +171,19 @@ public void setSensorInverted(final boolean inverted) { } @Override - public void setOutputRange(final double minOutput, final double maxOutput) { + public void setOutputRange( + ValueProvider minOutput, ValueProvider maxOutput, int slot) { + pidSlotHelper.setOutputRange(minOutput, maxOutput, slot); + } + + @Override + public void setOutputRange_Impl(final double minOutput, final double maxOutput, int slot) { + if (slot != 0) { + Logger.get(Category.HAL) + .logRaw( + Severity.WARNING, + "Ignoring slot for setOutputRange - unsupported on Talon"); + } errorCodeToException(ExceptionTarget.LOG, m_device.configPeakOutputReverse(minOutput)); errorCodeToException(ExceptionTarget.LOG, m_device.configPeakOutputForward(maxOutput)); } diff --git a/src/main/java/com/team766/hal/wpilib/CANVictorMotorController.java b/src/main/java/com/team766/hal/wpilib/CANVictorMotorController.java index 27cc3b3cf..6b35b6643 100644 --- a/src/main/java/com/team766/hal/wpilib/CANVictorMotorController.java +++ b/src/main/java/com/team766/hal/wpilib/CANVictorMotorController.java @@ -6,22 +6,29 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; import com.team766.hal.MotorController; +import com.team766.hal.PIDSlotHelper; +import com.team766.library.ValueProvider; import com.team766.logging.Category; import com.team766.logging.Logger; import com.team766.logging.LoggerExceptionUtils; import com.team766.logging.Severity; -public class CANVictorMotorController extends BaseCTREMotorController implements MotorController { +public class CANVictorMotorController extends BaseCTREMotorController + implements MotorController, PIDSlotHelper.MotorCallbacks { + private static final int NUM_PID_SLOTS = 2; - private WPI_VictorSPX m_device; - private double m_feedForward = 0.0; + private final WPI_VictorSPX m_device; + private final PIDSlotHelper pidSlotHelper; public CANVictorMotorController(final int deviceNumber) { m_device = new WPI_VictorSPX(deviceNumber); + pidSlotHelper = new PIDSlotHelper(this); } @Override - public void set(final ControlMode mode, double value) { + public void set(final ControlMode mode, double value, int slot, double arbitraryrFeedForward) { + m_device.selectProfileSlot(slot, 0 /* primary closed loop */); + com.ctre.phoenix.motorcontrol.ControlMode ctre_mode = null; boolean useFourTermSet = true; switch (mode) { @@ -56,7 +63,7 @@ public void set(final ControlMode mode, double value) { ctre_mode = com.ctre.phoenix.motorcontrol.ControlMode.Disabled; } if (useFourTermSet) { - m_device.set(ctre_mode, value, DemandType.ArbitraryFeedForward, m_feedForward); + m_device.set(ctre_mode, value, DemandType.ArbitraryFeedForward, arbitraryrFeedForward); } else { m_device.set(ctre_mode, value); } @@ -110,8 +117,13 @@ public void setClosedLoopRamp(final double secondsFromNeutralToFull) { } @Override - public void setFF(final double value) { - errorCodeToException(ExceptionTarget.LOG, m_device.config_kF(0, value, TIMEOUT_MS)); + public void setFF(final ValueProvider value, int slot) { + pidSlotHelper.setFF(value, slot); + } + + @Override + public void setFF_Impl(final double value, int slot) { + errorCodeToException(ExceptionTarget.LOG, m_device.config_kF(slot, value, TIMEOUT_MS)); } @Override @@ -140,18 +152,38 @@ public void setNeutralMode(final NeutralMode neutralMode) { } @Override - public void setP(final double value) { - errorCodeToException(ExceptionTarget.LOG, m_device.config_kP(0, value, TIMEOUT_MS)); + public int numPIDSlots() { + return NUM_PID_SLOTS; } @Override - public void setI(final double value) { - errorCodeToException(ExceptionTarget.LOG, m_device.config_kI(0, value, TIMEOUT_MS)); + public void setP(final ValueProvider value, int slot) { + pidSlotHelper.setP(value, slot); } @Override - public void setD(final double value) { - errorCodeToException(ExceptionTarget.LOG, m_device.config_kD(0, value, TIMEOUT_MS)); + public void setP_Impl(final double value, int slot) { + errorCodeToException(ExceptionTarget.LOG, m_device.config_kP(slot, value, TIMEOUT_MS)); + } + + @Override + public void setI(final ValueProvider value, int slot) { + pidSlotHelper.setI(value, slot); + } + + @Override + public void setI_Impl(final double value, int slot) { + errorCodeToException(ExceptionTarget.LOG, m_device.config_kI(slot, value, TIMEOUT_MS)); + } + + @Override + public void setD(final ValueProvider value, int slot) { + pidSlotHelper.setD(value, slot); + } + + @Override + public void setD_Impl(final double value, int slot) { + errorCodeToException(ExceptionTarget.LOG, m_device.config_kD(slot, value, TIMEOUT_MS)); } @Override @@ -166,7 +198,19 @@ public void setSensorInverted(final boolean inverted) { } @Override - public void setOutputRange(final double minOutput, final double maxOutput) { + public void setOutputRange( + ValueProvider minOutput, ValueProvider maxOutput, int slot) { + pidSlotHelper.setOutputRange(minOutput, maxOutput, slot); + } + + @Override + public void setOutputRange_Impl(final double minOutput, final double maxOutput, int slot) { + if (slot != 0) { + Logger.get(Category.HAL) + .logRaw( + Severity.WARNING, + "Ignoring slot for setOutputRange - unsupported on Talon"); + } errorCodeToException(ExceptionTarget.LOG, m_device.configPeakOutputReverse(minOutput)); errorCodeToException(ExceptionTarget.LOG, m_device.configPeakOutputForward(maxOutput)); } diff --git a/src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java b/src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java index 72a5f78c3..d60368211 100755 --- a/src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java +++ b/src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java @@ -131,9 +131,7 @@ public MotorController getMotor( motor = new CANSparkMaxMotorController(index); } catch (Exception ex) { LoggerExceptionUtils.logException(ex); - motor = - new LocalMotorController( - configPrefix, new MockMotorController(index), localSensor); + motor = new LocalMotorController(new MockMotorController(index), localSensor); localSensor = null; } break; @@ -149,7 +147,7 @@ public MotorController getMotor( motor = new CANTalonFxMotorController(index, getStringOrEmpty(canBus)); break; case VictorSP: - motor = new LocalMotorController(configPrefix, new PWMVictorSP(index), localSensor); + motor = new LocalMotorController(new PWMVictorSP(index), localSensor); localSensor = null; break; default: @@ -158,13 +156,11 @@ public MotorController getMotor( if (motor == null) { LoggerExceptionUtils.logException( new IllegalArgumentException("Unsupported motor type " + type)); - motor = - new LocalMotorController( - configPrefix, new MockMotorController(index), localSensor); + motor = new LocalMotorController(new MockMotorController(index), localSensor); localSensor = null; } if (localSensor != null) { - motor = new LocalMotorController(configPrefix, motor, localSensor); + motor = new LocalMotorController(motor, localSensor); } motors[type.ordinal()][index] = motor; return motor; diff --git a/src/main/java/com/team766/library/AbstractObservable.java b/src/main/java/com/team766/library/AbstractObservable.java new file mode 100644 index 000000000..263c37de9 --- /dev/null +++ b/src/main/java/com/team766/library/AbstractObservable.java @@ -0,0 +1,40 @@ +package com.team766.library; + +import java.util.LinkedList; +import java.util.List; +import java.util.Objects; + +/** + * A generic base class to aid in implementing the Observable interface. + * + * Classes that derive from this should call notifyObservers when their value changes. + */ +public class AbstractObservable implements Observable { + private List> _observers = new LinkedList>(); + + @Override + public void addObserver(Observer obs) { + Objects.requireNonNull(obs); + if (_observers.contains(obs)) { + return; + } + _observers.add(obs); + } + + @Override + public void removeObserver(Observer obs) { + _observers.remove(obs); + } + + /** + * Notify all observers that have been registered with this Observable that its value has + * changed. + * + * @param value The new value of this Observable. + */ + protected void notifyObservers(ValueType value) { + for (Observer obs : _observers) { + obs.onValueUpdated(value); + } + } +} diff --git a/src/main/java/com/team766/library/Observable.java b/src/main/java/com/team766/library/Observable.java new file mode 100644 index 000000000..3a491af55 --- /dev/null +++ b/src/main/java/com/team766/library/Observable.java @@ -0,0 +1,19 @@ +package com.team766.library; + +/** + * An Observable object can have one or more observers. An observer may be any object that + * implements the Observer interface. When an Observable instance changes its value, it will notify + * its observers of that change by a call to their onValueUpdated. + */ +public interface Observable { + /** + * Register an Observer to be notified of changes to this Observable object. + */ + public void addObserver(Observer obs); + + /** + * De-register an Observer that was previously passed to addObserver. This Observer will no + * longer receive notifications about this Observable object. + */ + public void removeObserver(Observer obs); +} diff --git a/src/main/java/com/team766/library/ObserveValue.java b/src/main/java/com/team766/library/ObserveValue.java new file mode 100644 index 000000000..a6c12264b --- /dev/null +++ b/src/main/java/com/team766/library/ObserveValue.java @@ -0,0 +1,64 @@ +package com.team766.library; + +import java.util.Optional; + +/** + * This class aids an Observer which may wish to subscribe a succession of different ValueProviders. + * + * This class will handle registering and de-registering the Observer as setValueProvider is called + * with different ValueProviders, to ensure that the Observer receives change notifications only + * from the current ValueProvider. Additionally, the Observer will be notifed when changing to a + * different ValueProvider, to indicate that the current value (as considered across the multiple + * ValueProviders) may have changed. Note that this notification also happens when setting the + * initial ValueProvider. + */ +public class ObserveValue { + private final Observer> m_observer; + private ValueProvider m_provider = null; + + public static Observer> whenPresent(Observer delegate) { + return (optValue) -> { + if (optValue.isPresent()) delegate.onValueUpdated(optValue.get()); + }; + } + + public ObserveValue(Observer> observer) { + m_observer = observer; + } + + /** + * Construct the ObserveValue with an initial ValueProvider to register with. + * This is equivalent to constructing this ObserveValue and then immediately calling + * setValueProvider. + */ + public ObserveValue(ValueProvider provider, Observer> observer) { + m_observer = observer; + setValueProvider(provider); + } + + /** + * Get the ValueProvider that was most recently passed to setValueProvider (or the constructor + * of this ObserveValue). Returns null if no ValueProvider has been set yet. + */ + public ValueProvider getValueProvider() { + return m_provider; + } + + /** + * Change the Observer so that it is registered to the given ValueProvider (instead of the + * previous one). Also notifies the Observer with the value of the new ValueProvider. + */ + public void setValueProvider(ValueProvider provider) { + if (m_provider != null) { + m_provider.removeObserver(m_observer); + } + m_provider = provider; + if (m_provider != null) { + m_provider.addObserver(m_observer); + } + m_observer.onValueUpdated( + m_provider != null && m_provider.hasValue() + ? Optional.of(m_provider.get()) + : Optional.empty()); + } +} diff --git a/src/main/java/com/team766/library/Observer.java b/src/main/java/com/team766/library/Observer.java new file mode 100644 index 000000000..4c5de5d62 --- /dev/null +++ b/src/main/java/com/team766/library/Observer.java @@ -0,0 +1,15 @@ +package com.team766.library; + +/** + * A class can implement the Observer interface when it wants to be informed of changes to + * Observable objects. + */ +@FunctionalInterface +public interface Observer { + /** + * This method is called whenever the observed object is changed. + * + * @param value The new value of the Observable object. + */ + public void onValueUpdated(ValueType value); +} diff --git a/src/main/java/com/team766/library/SetValueProvider.java b/src/main/java/com/team766/library/SetValueProvider.java index 8b03767f0..52e6bcce3 100644 --- a/src/main/java/com/team766/library/SetValueProvider.java +++ b/src/main/java/com/team766/library/SetValueProvider.java @@ -1,36 +1,41 @@ package com.team766.library; -public class SetValueProvider implements SettableValueProvider { - private E m_value; - private boolean m_hasValue; - - public SetValueProvider() { - m_value = null; - m_hasValue = false; - } - - public SetValueProvider(final E value) { - m_value = value; - m_hasValue = true; - } - - @Override - public E get() { - return m_value; - } - - @Override - public boolean hasValue() { - return m_hasValue; - } - - public void set(final E value) { - m_value = value; - m_hasValue = true; - } - - public void clear() { - m_value = null; - m_hasValue = false; - } +import java.util.Optional; + +public class SetValueProvider extends AbstractObservable> + implements SettableValueProvider { + private E m_value; + private boolean m_hasValue; + + public SetValueProvider() { + m_value = null; + m_hasValue = false; + } + + public SetValueProvider(final E value) { + m_value = value; + m_hasValue = true; + } + + @Override + public E get() { + return m_value; + } + + @Override + public boolean hasValue() { + return m_hasValue; + } + + public void set(final E value) { + m_value = value; + m_hasValue = true; + notifyObservers(Optional.of(m_value)); + } + + public void clear() { + m_value = null; + m_hasValue = false; + notifyObservers(Optional.empty()); + } } diff --git a/src/main/java/com/team766/library/TransformingValueProvider.java b/src/main/java/com/team766/library/TransformingValueProvider.java new file mode 100644 index 000000000..2b17805ae --- /dev/null +++ b/src/main/java/com/team766/library/TransformingValueProvider.java @@ -0,0 +1,39 @@ +package com.team766.library; + +import java.util.Optional; +import java.util.function.Function; + +/** + * This class implements a ValueProvider whose value (and the presence of its value) is based + * another ValueProvider, but the value from the underlying ValueProvided is transformed using the + * given transform function. + */ +public final class TransformingValueProvider extends AbstractObservable> + implements ValueProvider { + private Optional m_cachedValue; + + /** + * @param source The underlying ValueProvider which will provide the values passed to the + * transform function. + * @param transform The transform function applied to the values. + */ + public TransformingValueProvider(ValueProvider source, Function transform) { + source.addObserver( + optValue -> { + m_cachedValue = optValue.map(value -> transform.apply(value)); + notifyObservers(m_cachedValue); + }); + m_cachedValue = + source.hasValue() ? Optional.of(transform.apply(source.get())) : Optional.empty(); + } + + @Override + public E get() { + return m_cachedValue.get(); + } + + @Override + public boolean hasValue() { + return m_cachedValue.isPresent(); + } +} diff --git a/src/main/java/com/team766/library/ValueProvider.java b/src/main/java/com/team766/library/ValueProvider.java index 1d46c2ead..d61d58c64 100644 --- a/src/main/java/com/team766/library/ValueProvider.java +++ b/src/main/java/com/team766/library/ValueProvider.java @@ -1,14 +1,16 @@ package com.team766.library; -public interface ValueProvider { - E get(); - - boolean hasValue(); - - default E valueOr(E default_value) { - if (hasValue()) { - return get(); - } - return default_value; - } -} \ No newline at end of file +import java.util.Optional; + +public interface ValueProvider extends Observable> { + E get(); + + boolean hasValue(); + + default E valueOr(E default_value) { + if (hasValue()) { + return get(); + } + return default_value; + } +} diff --git a/src/main/java/com/team766/web/ConfigUI.java b/src/main/java/com/team766/web/ConfigUI.java index c1b56fb8d..a153561cd 100644 --- a/src/main/java/com/team766/web/ConfigUI.java +++ b/src/main/java/com/team766/web/ConfigUI.java @@ -2,10 +2,27 @@ import com.team766.config.ConfigFileReader; import com.team766.config.ConfigValueParseException; +import com.team766.framework.Scheduler; import java.util.ArrayList; +import java.util.Date; import java.util.Map; +import java.util.concurrent.ExecutionException; +import java.util.concurrent.FutureTask; +import java.util.concurrent.RunnableFuture; public class ConfigUI implements WebServer.Handler { + private RunnableFuture configToApply = null; + + public ConfigUI() { + Scheduler.getInstance() + .add( + () -> { + if (configToApply != null && !configToApply.isDone()) { + configToApply.run(); + } + }); + } + @Override public String endpoint() { return "/config"; @@ -19,18 +36,25 @@ public String handle(Map params) { if (params.containsKey("configJson")) { String configJsonString = (String) params.get("configJson"); ArrayList validationErrors = new ArrayList(); + configToApply = + new FutureTask( + () -> { + try { + ConfigFileReader.getInstance().reloadFromJson(configJsonString); + } catch (ConfigValueParseException ex) { + validationErrors.add(ex.toString()); + } catch (Exception ex) { + validationErrors.add("Failed to parse config json: " + ex); + } + return null; + }); try { - ConfigFileReader.getInstance().reloadFromJson(configJsonString); - } catch (ConfigValueParseException ex) { + configToApply.get(); + } catch (InterruptedException | ExecutionException ex) { validationErrors.add(ex.toString()); - } catch (Exception ex) { - validationErrors.add("Failed to parse config json: " + ex); } if (validationErrors.isEmpty()) { - r += - "

New configuration (v" - + ConfigFileReader.getInstance().getGeneration() - + ") has been applied

"; + r += "

New configuration (" + new Date().toString() + ") has been applied

"; r += "

Remember to click Restart Robot Code in the driver station if you have changed any motor/sensor settings

"; if (params.containsKey("saveToFile")) { diff --git a/src/test/java/com/team766/config/ConfigFileReaderTest.java b/src/test/java/com/team766/config/ConfigFileReaderTest.java index 9a6b3531c..90ea16b5c 100644 --- a/src/test/java/com/team766/config/ConfigFileReaderTest.java +++ b/src/test/java/com/team766/config/ConfigFileReaderTest.java @@ -1,43 +1,115 @@ package com.team766.config; -import static org.junit.Assert.*; +import static org.junit.jupiter.api.Assertions.*; + import java.io.File; import java.io.FileWriter; import java.io.IOException; -import org.junit.Before; -import org.junit.Test; +import java.util.Optional; +import java.util.concurrent.atomic.AtomicReference; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; public class ConfigFileReaderTest { - @Before - public void setup() { - AbstractConfigValue.resetStatics(); - } - - @Test - public void getJsonStringFromEmptyConfigFile() throws IOException { - File testConfigFile = File.createTempFile("config_file_test", ".json"); - try (FileWriter fos = new FileWriter(testConfigFile)) { - fos.append("{}"); - } - - ConfigFileReader.instance = new ConfigFileReader(testConfigFile.getPath()); - ConfigFileReader.getInstance().getString("test.sub.key"); - assertEquals("{\"test\": {\"sub\": {\"key\": null}}}", ConfigFileReader.getInstance().getJsonString()); - } - - @Test - public void getJsonStringFromPartialConfigFile() throws IOException { - File testConfigFile = File.createTempFile("config_file_test", ".json"); - try (FileWriter fos = new FileWriter(testConfigFile)) { - fos.append("{\"test\": {\"sub\": {\"key\": \"pi\", \"value\": 3.14159}}}"); - } - - ConfigFileReader.instance = new ConfigFileReader(testConfigFile.getPath()); - assertEquals("pi", ConfigFileReader.getInstance().getString("test.sub.key").get()); - assertEquals(3.14159, ConfigFileReader.getInstance().getDouble("test.sub.value").get().doubleValue(), 1e-6); - assertFalse(ConfigFileReader.getInstance().getInts("test.other.value").hasValue()); - assertEquals( - "{\"test\": {\n \"sub\": {\n \"value\": 3.14159,\n \"key\": \"pi\"\n },\n \"other\": {\"value\": null}\n}}", - ConfigFileReader.getInstance().getJsonString()); - } -} \ No newline at end of file + @BeforeEach + public void setup() { + AbstractConfigValue.resetStatics(); + } + + @Test + public void isPrefix() { + assertTrue(ConfigFileReader.isPrefix(new String[] {}, new String[] {})); + assertTrue(ConfigFileReader.isPrefix(new String[] {}, new String[] {"a"})); + assertFalse(ConfigFileReader.isPrefix(new String[] {"a"}, new String[] {})); + assertTrue(ConfigFileReader.isPrefix(new String[] {"a"}, new String[] {"a"})); + assertTrue(ConfigFileReader.isPrefix(new String[] {"a"}, new String[] {"a", "b"})); + assertTrue(ConfigFileReader.isPrefix(new String[] {"a", "b"}, new String[] {"a", "b"})); + assertFalse(ConfigFileReader.isPrefix(new String[] {"a", "b"}, new String[] {"a"})); + } + + @Test + public void getJsonStringFromEmptyConfigFile() throws IOException { + File testConfigFile = File.createTempFile("config_file_test", ".json"); + try (FileWriter fos = new FileWriter(testConfigFile)) { + fos.append("{}"); + } + + ConfigFileReader.instance = new ConfigFileReader(testConfigFile.getPath()); + ConfigFileReader.getInstance().getString("test.sub.key"); + assertEquals( + "{\"test\": {\"sub\": {\"key\": null}}}", + ConfigFileReader.getInstance().getJsonString()); + } + + @Test + public void getJsonStringFromPartialConfigFile() throws IOException { + File testConfigFile = File.createTempFile("config_file_test", ".json"); + try (FileWriter fos = new FileWriter(testConfigFile)) { + fos.append("{\"test\": {\"sub\": {\"key\": \"pi\", \"value\": 3.14159}}}"); + } + + ConfigFileReader.instance = new ConfigFileReader(testConfigFile.getPath()); + assertEquals("pi", ConfigFileReader.getInstance().getString("test.sub.key").get()); + assertEquals( + 3.14159, + ConfigFileReader.getInstance().getDouble("test.sub.value").get().doubleValue(), + 1e-6); + assertFalse(ConfigFileReader.getInstance().getInts("test.other.value").hasValue()); + assertEquals( + "{\"test\": {\n \"sub\": {\n \"value\": 3.14159,\n \"key\": \"pi\"\n },\n \"other\": {\"value\": null}\n}}", + ConfigFileReader.getInstance().getJsonString()); + } + + @Test + public void observeChangeInConfigFile() throws IOException { + File testConfigFile = File.createTempFile("config_file_test", ".json"); + try (FileWriter fos = new FileWriter(testConfigFile)) { + fos.append("{\"test\": {\"sub\": {\"key\": \"pi\", \"value\": 3.14159}}}"); + } + + ConfigFileReader.instance = new ConfigFileReader(testConfigFile.getPath()); + var keyProvider = ConfigFileReader.getInstance().getString("test.sub.key"); + var valueProvider = ConfigFileReader.getInstance().getDouble("test.sub.value"); + + final AtomicReference> lastKeyUpdate = new AtomicReference<>(); + keyProvider.addObserver(lastKeyUpdate::set); + final AtomicReference> lastValueUpdate = new AtomicReference<>(); + valueProvider.addObserver(lastValueUpdate::set); + + assertNull(lastKeyUpdate.get()); + assertNull(lastValueUpdate.get()); + + ConfigFileReader.getInstance() + .reloadFromJson("{\"test\": {\"sub\": {\"key\": \"tau\", \"value\": 6.28319}}}"); + assertEquals("tau", keyProvider.get()); + assertEquals("tau", lastKeyUpdate.get().get()); + assertEquals(6.28319, valueProvider.get().doubleValue(), 1e-6); + assertEquals(6.28319, lastValueUpdate.get().get().doubleValue(), 1e-6); + } + + @Test + public void observeConfigValueMutation() throws IOException { + File testConfigFile = File.createTempFile("config_file_test", ".json"); + try (FileWriter fos = new FileWriter(testConfigFile)) { + fos.append("{\"test\": {\"sub\": {\"key\": \"pi\", \"value\": 3.14159}}}"); + } + + ConfigFileReader.instance = new ConfigFileReader(testConfigFile.getPath()); + + var providerReader = ConfigFileReader.getInstance().getString("test.sub.key"); + final AtomicReference> readerLastUpdate = new AtomicReference<>(); + providerReader.addObserver(readerLastUpdate::set); + + var providerWriter = ConfigFileReader.getInstance().getString("test.sub.key"); + final AtomicReference> writerLastUpdate = new AtomicReference<>(); + providerWriter.addObserver(writerLastUpdate::set); + + providerWriter.set("gamma"); + assertEquals(Optional.of("gamma"), readerLastUpdate.get()); + assertEquals(Optional.of("gamma"), writerLastUpdate.get()); + + providerWriter.clear(); + assertEquals(Optional.empty(), readerLastUpdate.get()); + assertEquals(Optional.empty(), writerLastUpdate.get()); + } +} diff --git a/src/test/java/com/team766/library/ObserveValueTest.java b/src/test/java/com/team766/library/ObserveValueTest.java new file mode 100644 index 000000000..a8bca9a16 --- /dev/null +++ b/src/test/java/com/team766/library/ObserveValueTest.java @@ -0,0 +1,50 @@ +package com.team766.library; + +import static org.junit.jupiter.api.Assertions.*; + +import java.util.Optional; +import java.util.concurrent.atomic.AtomicReference; +import org.junit.jupiter.api.Test; + +public class ObserveValueTest { + @Test + public void testNotificationFromValueProvider() { + var provider = new SetValueProvider(10); + + final AtomicReference> lastUpdate = new AtomicReference<>(); + @SuppressWarnings("unused") + ObserveValue observe = new ObserveValue<>(provider, lastUpdate::set); + + // Test notification from initial value provider. + assertEquals(lastUpdate.get(), Optional.of(10)); + + // Test notification from setting a new value in the provider. + provider.set(20); + assertEquals(lastUpdate.get(), Optional.of(20)); + + // Test notification from clearing the value in the provider. + provider.clear(); + assertEquals(lastUpdate.get(), Optional.empty()); + + // Test notification from re-setting the value in the provider. + provider.set(30); + assertEquals(lastUpdate.get(), Optional.of(30)); + } + + @Test + public void testNotificationFromChangingValueProviders() { + final AtomicReference> lastUpdate = new AtomicReference<>(); + ObserveValue observe = new ObserveValue<>(lastUpdate::set); + + assertNull(lastUpdate.get()); + + observe.setValueProvider(new SetValueProvider(10)); + assertEquals(lastUpdate.get(), Optional.of(10)); + + observe.setValueProvider(new SetValueProvider()); + assertEquals(lastUpdate.get(), Optional.empty()); + + observe.setValueProvider(new SetValueProvider(20)); + assertEquals(lastUpdate.get(), Optional.of(20)); + } +} diff --git a/src/test/java/com/team766/library/TransformingValueProviderTest.java b/src/test/java/com/team766/library/TransformingValueProviderTest.java new file mode 100644 index 000000000..2f422cd0f --- /dev/null +++ b/src/test/java/com/team766/library/TransformingValueProviderTest.java @@ -0,0 +1,51 @@ +package com.team766.library; + +import static org.junit.jupiter.api.Assertions.*; + +import java.util.Optional; +import java.util.concurrent.atomic.AtomicReference; +import org.junit.jupiter.api.Test; + +public class TransformingValueProviderTest { + @Test + public void testValue() { + var sourceProvider = new SetValueProvider(10); + + var xfProvider = new TransformingValueProvider<>(sourceProvider, v -> -v); + assertTrue(xfProvider.hasValue()); + assertEquals(xfProvider.get(), -10); + + sourceProvider.set(20); + assertTrue(xfProvider.hasValue()); + assertEquals(xfProvider.get(), -20); + + sourceProvider.clear(); + assertFalse(xfProvider.hasValue()); + + var xfProvider2 = new TransformingValueProvider<>(sourceProvider, v -> -v); + assertFalse(xfProvider2.hasValue()); + } + + @Test + public void testObservation() { + var sourceProvider = new SetValueProvider(10); + + var xfProvider = new TransformingValueProvider<>(sourceProvider, v -> -v); + + final AtomicReference> lastUpdate = new AtomicReference<>(); + Observer> observer = lastUpdate::set; + xfProvider.addObserver(observer); + + assertNull(lastUpdate.get()); + + sourceProvider.set(20); + assertEquals(lastUpdate.get(), Optional.of(-20)); + + sourceProvider.clear(); + assertEquals(lastUpdate.get(), Optional.empty()); + + xfProvider.removeObserver(observer); + sourceProvider.set(30); + assertEquals(lastUpdate.get(), Optional.empty()); + } +}