From b670422c8e788a9d626a997ef7b35c66bad6288a Mon Sep 17 00:00:00 2001 From: Sam Carlberg Date: Thu, 16 Feb 2023 18:06:47 -0500 Subject: [PATCH] Add a widget for ProfiledPIDController Effectively identical to PIDController for now, but future updates to WPILib will send more information --- .../shuffleboard/plugin/base/BasePlugin.java | 7 +- .../base/data/ProfiledPIDControllerData.java | 120 ++++++++++++++++++ .../data/types/ProfiledPIDControllerType.java | 27 ++++ .../widget/ProfiledPIDControllerWidget.java | 84 ++++++++++++ .../widget/ProfiledPIDControllerWidget.fxml | 29 +++++ 5 files changed, 266 insertions(+), 1 deletion(-) create mode 100644 plugins/base/src/main/java/edu/wpi/first/shuffleboard/plugin/base/data/ProfiledPIDControllerData.java create mode 100644 plugins/base/src/main/java/edu/wpi/first/shuffleboard/plugin/base/data/types/ProfiledPIDControllerType.java create mode 100644 plugins/base/src/main/java/edu/wpi/first/shuffleboard/plugin/base/widget/ProfiledPIDControllerWidget.java create mode 100644 plugins/base/src/main/resources/edu/wpi/first/shuffleboard/plugin/base/widget/ProfiledPIDControllerWidget.fxml diff --git a/plugins/base/src/main/java/edu/wpi/first/shuffleboard/plugin/base/BasePlugin.java b/plugins/base/src/main/java/edu/wpi/first/shuffleboard/plugin/base/BasePlugin.java index 29aa74f55..9d488112d 100644 --- a/plugins/base/src/main/java/edu/wpi/first/shuffleboard/plugin/base/BasePlugin.java +++ b/plugins/base/src/main/java/edu/wpi/first/shuffleboard/plugin/base/BasePlugin.java @@ -28,6 +28,7 @@ import edu.wpi.first.shuffleboard.plugin.base.data.types.MecanumDriveType; import edu.wpi.first.shuffleboard.plugin.base.data.types.PIDCommandType; import edu.wpi.first.shuffleboard.plugin.base.data.types.PIDControllerType; +import edu.wpi.first.shuffleboard.plugin.base.data.types.ProfiledPIDControllerType; import edu.wpi.first.shuffleboard.plugin.base.data.types.PowerDistributionType; import edu.wpi.first.shuffleboard.plugin.base.data.types.QuadratureEncoderType; import edu.wpi.first.shuffleboard.plugin.base.data.types.RelayType; @@ -57,6 +58,7 @@ import edu.wpi.first.shuffleboard.plugin.base.widget.NumberSliderWidget; import edu.wpi.first.shuffleboard.plugin.base.widget.PIDCommandWidget; import edu.wpi.first.shuffleboard.plugin.base.widget.PIDControllerWidget; +import edu.wpi.first.shuffleboard.plugin.base.widget.ProfiledPIDControllerWidget; import edu.wpi.first.shuffleboard.plugin.base.widget.PowerDistributionPanelWidget; import edu.wpi.first.shuffleboard.plugin.base.widget.RelayWidget; import edu.wpi.first.shuffleboard.plugin.base.widget.RobotPreferencesWidget; @@ -79,7 +81,7 @@ @Description( group = "edu.wpi.first.shuffleboard", name = "Base", - version = "1.3.4", + version = "1.3.5", summary = "Defines all the WPILib data types and stock widgets" ) @SuppressWarnings("PMD.CouplingBetweenObjects") @@ -118,6 +120,7 @@ public List getDataTypes() { CommandType.Instance, PIDCommandType.Instance, PIDControllerType.Instance, + ProfiledPIDControllerType.Instance, AccelerometerType.Instance, ThreeAxisAccelerometerType.Instance, GyroType.Instance, @@ -154,6 +157,7 @@ public List getComponents() { WidgetType.forAnnotatedWidget(AccelerometerWidget.class), WidgetType.forAnnotatedWidget(ThreeAxisAccelerometerWidget.class), WidgetType.forAnnotatedWidget(PIDControllerWidget.class), + WidgetType.forAnnotatedWidget(ProfiledPIDControllerWidget.class), WidgetType.forAnnotatedWidget(GyroWidget.class), WidgetType.forAnnotatedWidget(RelayWidget.class), WidgetType.forAnnotatedWidget(DifferentialDriveWidget.class), @@ -183,6 +187,7 @@ public Map getDefaultComponents() { .put(CommandType.Instance, WidgetType.forAnnotatedWidget(CommandWidget.class)) .put(PIDCommandType.Instance, WidgetType.forAnnotatedWidget(PIDCommandWidget.class)) .put(PIDControllerType.Instance, WidgetType.forAnnotatedWidget(PIDControllerWidget.class)) + .put(ProfiledPIDControllerType.Instance, WidgetType.forAnnotatedWidget(ProfiledPIDControllerWidget.class)) .put(AccelerometerType.Instance, WidgetType.forAnnotatedWidget(AccelerometerWidget.class)) .put(ThreeAxisAccelerometerType.Instance, WidgetType.forAnnotatedWidget(ThreeAxisAccelerometerWidget.class)) .put(GyroType.Instance, WidgetType.forAnnotatedWidget(GyroWidget.class)) diff --git a/plugins/base/src/main/java/edu/wpi/first/shuffleboard/plugin/base/data/ProfiledPIDControllerData.java b/plugins/base/src/main/java/edu/wpi/first/shuffleboard/plugin/base/data/ProfiledPIDControllerData.java new file mode 100644 index 000000000..2515bd32a --- /dev/null +++ b/plugins/base/src/main/java/edu/wpi/first/shuffleboard/plugin/base/data/ProfiledPIDControllerData.java @@ -0,0 +1,120 @@ +package edu.wpi.first.shuffleboard.plugin.base.data; + +import com.google.common.collect.ImmutableMap; +import edu.wpi.first.shuffleboard.api.data.ComplexData; + +import java.util.Map; +import java.util.Objects; + +/** + * Data for a profiled PID controller from WPILib. WPILib currently sends P, I, D, and the goal position; this makes + * it effectively identical to {@link PIDControllerData}. Future updates to WPILib to fully support profiled PID + * controllers will result in extra fields in the data; for example, the goal velocity or trapezoidal constraints. + */ +public final class ProfiledPIDControllerData extends ComplexData { + + private final double p; + private final double i; + private final double d; + private final double goal; + + /** + * Creates a new PIDController data object. + * + * @param p the proportional constant + * @param i the integral constant + * @param d the derivative constant + * @param goal the controller goal + */ + public ProfiledPIDControllerData(double p, double i, double d, double goal) { + this.p = p; + this.i = i; + this.d = d; + this.goal = goal; + } + + /** + * Creates a new data object from a map. The map should contain values for all the properties of the data object. If + * a value is missing, the default value of {@code 0} (for numbers) is used. + */ + public ProfiledPIDControllerData(Map map) { + this((double) map.getOrDefault("p", 0.0), + (double) map.getOrDefault("i", 0.0), + (double) map.getOrDefault("d", 0.0), + (double) map.getOrDefault("goal", 0.0)); + } + + public double getP() { + return p; + } + + public double getI() { + return i; + } + + public double getD() { + return d; + } + + public double getGoal() { + return goal; + } + + + public ProfiledPIDControllerData withP(double p) { + return new ProfiledPIDControllerData(p, i, d, goal); + } + + public ProfiledPIDControllerData withI(double i) { + return new ProfiledPIDControllerData(p, i, d, goal); + } + + public ProfiledPIDControllerData withD(double d) { + return new ProfiledPIDControllerData(p, i, d, goal); + } + + public ProfiledPIDControllerData withGoal(double goal) { + return new ProfiledPIDControllerData(p, i, d, goal); + } + + @Override + public Map asMap() { + return ImmutableMap.builder() + .put("p", p) + .put("i", i) + .put("d", d) + .put("goal", goal) + .build(); + } + + @Override + public boolean equals(Object obj) { + if (this == obj) { + return true; + } + if (obj == null || getClass() != obj.getClass()) { + return false; + } + ProfiledPIDControllerData that = (ProfiledPIDControllerData) obj; + return this.p == that.p + && this.i == that.i + && this.d == that.d + && this.goal == that.goal; + } + + @Override + public int hashCode() { + return Objects.hash(p, i, d, goal); + } + + @Override + public String toString() { + return String.format("ProfiledPIDControllerData(p=%s, i=%s, d=%s, goal=%s)", + p, i, d, goal); + } + + @Override + public String toHumanReadableString() { + return String.format("p=%.3f, i=%.3f, d=%.3f, goal=%.3f", p, i, d, goal); + } +} diff --git a/plugins/base/src/main/java/edu/wpi/first/shuffleboard/plugin/base/data/types/ProfiledPIDControllerType.java b/plugins/base/src/main/java/edu/wpi/first/shuffleboard/plugin/base/data/types/ProfiledPIDControllerType.java new file mode 100644 index 000000000..2d428ccc2 --- /dev/null +++ b/plugins/base/src/main/java/edu/wpi/first/shuffleboard/plugin/base/data/types/ProfiledPIDControllerType.java @@ -0,0 +1,27 @@ +package edu.wpi.first.shuffleboard.plugin.base.data.types; + +import edu.wpi.first.shuffleboard.api.data.ComplexDataType; +import edu.wpi.first.shuffleboard.plugin.base.data.ProfiledPIDControllerData; + +import java.util.Map; +import java.util.function.Function; + +public final class ProfiledPIDControllerType extends ComplexDataType { + + public static final ProfiledPIDControllerType Instance = new ProfiledPIDControllerType(); + + private ProfiledPIDControllerType() { + super("ProfiledPIDController", ProfiledPIDControllerData.class); + } + + @Override + public Function, ProfiledPIDControllerData> fromMap() { + return ProfiledPIDControllerData::new; + } + + @Override + public ProfiledPIDControllerData getDefaultValue() { + return new ProfiledPIDControllerData(0, 0, 0, 0); + } + +} diff --git a/plugins/base/src/main/java/edu/wpi/first/shuffleboard/plugin/base/widget/ProfiledPIDControllerWidget.java b/plugins/base/src/main/java/edu/wpi/first/shuffleboard/plugin/base/widget/ProfiledPIDControllerWidget.java new file mode 100644 index 000000000..d6c9803ca --- /dev/null +++ b/plugins/base/src/main/java/edu/wpi/first/shuffleboard/plugin/base/widget/ProfiledPIDControllerWidget.java @@ -0,0 +1,84 @@ +package edu.wpi.first.shuffleboard.plugin.base.widget; + +import edu.wpi.first.shuffleboard.api.components.NumberField; +import edu.wpi.first.shuffleboard.api.widget.Description; +import edu.wpi.first.shuffleboard.api.widget.ParametrizedController; +import edu.wpi.first.shuffleboard.api.widget.SimpleAnnotatedWidget; +import edu.wpi.first.shuffleboard.plugin.base.data.ProfiledPIDControllerData; +import javafx.event.ActionEvent; +import javafx.fxml.FXML; +import javafx.scene.control.TextField; +import javafx.scene.layout.Pane; + +/** + * A widget for controlling Profiled PID controllers. This gives control over the three PID constants and the controller + * position goal. + * + *

Future updates to the sendable implementation for profiled PID controllers (such as support for goal velocity and + * trapezoidal constraints) will result in more available controls in this widget. + */ +@Description(name = "Profiled PID Controller", dataTypes = ProfiledPIDControllerData.class) +@ParametrizedController("ProfiledPIDControllerWidget.fxml") +public class ProfiledPIDControllerWidget extends SimpleAnnotatedWidget { + + @FXML + private Pane root; + @FXML + private NumberField pField; + @FXML + private NumberField iField; + @FXML + private NumberField dField; + @FXML + private NumberField goalField; + + @FXML + private void initialize() { + root.setStyle("-fx-font-size: 10pt;"); + dataProperty().addListener((__, old, newData) -> { + pField.setNumber(newData.getP()); + iField.setNumber(newData.getI()); + dField.setNumber(newData.getD()); + goalField.setNumber(newData.getGoal()); + }); + + actOnFocusLost(pField); + actOnFocusLost(iField); + actOnFocusLost(dField); + actOnFocusLost(goalField); + } + + private void actOnFocusLost(TextField field) { + field.focusedProperty().addListener((__, was, is) -> { + if (!is) { + field.getOnAction().handle(new ActionEvent(this, field)); + } + }); + } + + @Override + public Pane getView() { + return root; + } + + @FXML + private void setP() { + setData(getData().withP(pField.getNumber())); + } + + @FXML + private void setI() { + setData(getData().withI(iField.getNumber())); + } + + @FXML + private void setD() { + setData(getData().withD(dField.getNumber())); + } + + @FXML + private void setGoal() { + setData(getData().withGoal(goalField.getNumber())); + } + +} diff --git a/plugins/base/src/main/resources/edu/wpi/first/shuffleboard/plugin/base/widget/ProfiledPIDControllerWidget.fxml b/plugins/base/src/main/resources/edu/wpi/first/shuffleboard/plugin/base/widget/ProfiledPIDControllerWidget.fxml new file mode 100644 index 000000000..e030f01a4 --- /dev/null +++ b/plugins/base/src/main/resources/edu/wpi/first/shuffleboard/plugin/base/widget/ProfiledPIDControllerWidget.fxml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + +