From 3d2d35dc4412dfdedb4b59f022864ab7000aaf5c Mon Sep 17 00:00:00 2001 From: Vincent Wang Date: Fri, 31 Mar 2023 13:23:27 -0400 Subject: [PATCH 1/3] Make Controllers Sendable --- .../stuypulse/stuylib/control/Controller.java | 16 +++++++-- .../stuylib/control/ControllerGroup.java | 35 +++++++++++++++++-- .../control/feedback/PIDCalculator.java | 11 ++++++ .../control/feedback/PIDController.java | 11 ++++++ .../control/feedforward/ArmFeedforward.java | 11 ++++++ .../feedforward/ElevatorFeedforward.java | 11 ++++++ .../PositionFeedforwardController.java | 11 ++++++ .../VelocityFeedforwardController.java | 11 ++++++ 8 files changed, 113 insertions(+), 4 deletions(-) diff --git a/src/com/stuypulse/stuylib/control/Controller.java b/src/com/stuypulse/stuylib/control/Controller.java index b54ec934..477c475e 100644 --- a/src/com/stuypulse/stuylib/control/Controller.java +++ b/src/com/stuypulse/stuylib/control/Controller.java @@ -6,6 +6,9 @@ import com.stuypulse.stuylib.streams.filters.IFilter; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; + /** * A controller calculates an output variable given a setpoint and measurement of a single variable. * @@ -22,7 +25,7 @@ * * @author Myles Pasetsky (myles.pasetsky@gmail.com) */ -public abstract class Controller { +public abstract class Controller implements Sendable { /** The most recent setpoint of the controller */ private double mSetpoint; @@ -53,6 +56,15 @@ public Controller() { mOutputFilter = x -> x; } + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Controller"); + builder.addDoubleProperty("Setpoint", this::getSetpoint, null); + builder.addDoubleProperty("Measurement", this::getMeasurement, null); + builder.addDoubleProperty("Output", this::getOutput, null); + builder.addDoubleProperty("Error", this::getError, null); + } + /** * Set the setpoint filter of the controller * @@ -123,7 +135,7 @@ public final boolean isDone(double acceptableError) { * @param other the other controllers * @return the group of controllers that */ - public final ControllerGroup add(Controller... other) { + public ControllerGroup add(Controller... other) { return new ControllerGroup(this, other); } diff --git a/src/com/stuypulse/stuylib/control/ControllerGroup.java b/src/com/stuypulse/stuylib/control/ControllerGroup.java index 158f49ae..23dc3005 100644 --- a/src/com/stuypulse/stuylib/control/ControllerGroup.java +++ b/src/com/stuypulse/stuylib/control/ControllerGroup.java @@ -4,6 +4,11 @@ package com.stuypulse.stuylib.control; +import java.util.ArrayList; +import java.util.ResourceBundle.Control; + +import edu.wpi.first.util.sendable.SendableBuilder; + /** * Controllers can be grouped together in a "controller group" if they have the same setpoint and * measurement. @@ -19,12 +24,38 @@ public class ControllerGroup extends Controller { private final Controller mController; /** Controllers part of the group */ - private final Controller[] mControllers; + private final ArrayList mControllers; /** Create a controller group */ public ControllerGroup(Controller controller, Controller... controllers) { mController = controller; - mControllers = controllers; + mControllers = new ArrayList<>(); + + for (Controller tmpController : controllers) { + if (controller == null) { + throw new IllegalArgumentException("Controller cannot be null"); + } + mControllers.add(tmpController); + } + } + + @Override + public void initSendable(SendableBuilder builder) { + mController.initSendable(builder); + for (Controller controller : mControllers) { + controller.initSendable(builder); + } + } + + @Override + public ControllerGroup add(Controller... controller) { + for (Controller tmpController : controller) { + if (tmpController == null) { + throw new IllegalArgumentException("Controller cannot be null"); + } + mControllers.add(tmpController); + } + return this; } /** diff --git a/src/com/stuypulse/stuylib/control/feedback/PIDCalculator.java b/src/com/stuypulse/stuylib/control/feedback/PIDCalculator.java index 6130c3eb..1d8521d5 100644 --- a/src/com/stuypulse/stuylib/control/feedback/PIDCalculator.java +++ b/src/com/stuypulse/stuylib/control/feedback/PIDCalculator.java @@ -12,6 +12,8 @@ import com.stuypulse.stuylib.streams.filters.TimedMovingAverage; import com.stuypulse.stuylib.util.StopWatch; +import edu.wpi.first.util.sendable.SendableBuilder; + /** * This is a Bang-Bang controller that while controlling the robot, will be able to calculate the * values for the PID controller. It does this by taking the results of oscillations, then creating @@ -78,6 +80,15 @@ public PIDCalculator(Number speed) { mRunning = false; } + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("PID Calculator"); + builder.addDoubleProperty("Setpoint", this::getSetpoint, null); + builder.addDoubleProperty("Measurement", this::getMeasurement, null); + builder.addDoubleProperty("Output", this::getOutput, null); + builder.addDoubleProperty("Error", this::getError, null); + } + /** * @param speed sets speed for motor output of controller * @return the calculated result from the PIDController diff --git a/src/com/stuypulse/stuylib/control/feedback/PIDController.java b/src/com/stuypulse/stuylib/control/feedback/PIDController.java index f001a332..4fc8f70b 100644 --- a/src/com/stuypulse/stuylib/control/feedback/PIDController.java +++ b/src/com/stuypulse/stuylib/control/feedback/PIDController.java @@ -11,6 +11,8 @@ import com.stuypulse.stuylib.streams.filters.IFilterGroup; import com.stuypulse.stuylib.util.StopWatch; +import edu.wpi.first.util.sendable.SendableBuilder; + /** * The PID algorithm is a feedback control algorithm meant for calculating an output based on an * error between a measurement and a setpoint. @@ -66,6 +68,15 @@ public PIDController(Number p, Number i, Number d) { reset(); } + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("PID Controller"); + builder.addDoubleProperty("Setpoint", this::getSetpoint, null); + builder.addDoubleProperty("Measurement", this::getMeasurement, null); + builder.addDoubleProperty("Output", this::getOutput, null); + builder.addDoubleProperty("Error", this::getError, null); + } + /** Creates a blank PIDController that doesn't move */ public PIDController() { this(-1, -1, -1); diff --git a/src/com/stuypulse/stuylib/control/feedforward/ArmFeedforward.java b/src/com/stuypulse/stuylib/control/feedforward/ArmFeedforward.java index 9a2daf85..5af065d9 100644 --- a/src/com/stuypulse/stuylib/control/feedforward/ArmFeedforward.java +++ b/src/com/stuypulse/stuylib/control/feedforward/ArmFeedforward.java @@ -7,6 +7,8 @@ import com.stuypulse.stuylib.control.Controller; import com.stuypulse.stuylib.streams.filters.IFilter; +import edu.wpi.first.util.sendable.SendableBuilder; + /** * A feedforward term to account for gravity for motorized arms. * @@ -47,6 +49,15 @@ public ArmFeedforward(Number kG, IFilter cosine) { this.cosine = cosine; } + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Arm Feed Forward"); + builder.addDoubleProperty("Setpoint", this::getSetpoint, null); + builder.addDoubleProperty("Measurement", this::getMeasurement, null); + builder.addDoubleProperty("Output", this::getOutput, null); + builder.addDoubleProperty("Error", this::getError, null); + } + /** * Calculates voltage to hold arm at the setpoint angle * diff --git a/src/com/stuypulse/stuylib/control/feedforward/ElevatorFeedforward.java b/src/com/stuypulse/stuylib/control/feedforward/ElevatorFeedforward.java index c3ae54ba..9ca5a884 100644 --- a/src/com/stuypulse/stuylib/control/feedforward/ElevatorFeedforward.java +++ b/src/com/stuypulse/stuylib/control/feedforward/ElevatorFeedforward.java @@ -6,6 +6,8 @@ import com.stuypulse.stuylib.control.Controller; +import edu.wpi.first.util.sendable.SendableBuilder; + /** * A feedforward term to account for gravity for motorized lifts. * @@ -30,6 +32,15 @@ public ElevatorFeedforward(Number kG) { this.kG = kG; } + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Elevator Feed Forward"); + builder.addDoubleProperty("Setpoint", this::getSetpoint, null); + builder.addDoubleProperty("Measurement", this::getMeasurement, null); + builder.addDoubleProperty("Output", this::getOutput, null); + builder.addDoubleProperty("Error", this::getError, null); + } + /** * Calculate voltage to hold elevator at setpoint height * diff --git a/src/com/stuypulse/stuylib/control/feedforward/PositionFeedforwardController.java b/src/com/stuypulse/stuylib/control/feedforward/PositionFeedforwardController.java index da240005..6146ed7b 100644 --- a/src/com/stuypulse/stuylib/control/feedforward/PositionFeedforwardController.java +++ b/src/com/stuypulse/stuylib/control/feedforward/PositionFeedforwardController.java @@ -7,6 +7,8 @@ import com.stuypulse.stuylib.control.Controller; import com.stuypulse.stuylib.streams.filters.Derivative; +import edu.wpi.first.util.sendable.SendableBuilder; + /** * A positional controller that uses a feedforward model to calculate motor outputs given positional * setpoints. @@ -34,6 +36,15 @@ public PositionFeedforwardController(MotorFeedforward feedforward) { mFeedforward = feedforward; } + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Position Feed Forward Controller"); + builder.addDoubleProperty("Setpoint", this::getSetpoint, null); + builder.addDoubleProperty("Measurement", this::getMeasurement, null); + builder.addDoubleProperty("Output", this::getOutput, null); + builder.addDoubleProperty("Error", this::getError, null); + } + /** * Calculates a motor output by feeding the derivative of a positional setpoint to a feedforward * model diff --git a/src/com/stuypulse/stuylib/control/feedforward/VelocityFeedforwardController.java b/src/com/stuypulse/stuylib/control/feedforward/VelocityFeedforwardController.java index 03f72593..345ee999 100644 --- a/src/com/stuypulse/stuylib/control/feedforward/VelocityFeedforwardController.java +++ b/src/com/stuypulse/stuylib/control/feedforward/VelocityFeedforwardController.java @@ -6,6 +6,8 @@ import com.stuypulse.stuylib.control.Controller; +import edu.wpi.first.util.sendable.SendableBuilder; + /** * A velocity controller that uses a feedforward model to calculate motor outputs given velocity * setpoints. @@ -26,6 +28,15 @@ public VelocityFeedforwardController(MotorFeedforward feedforward) { mFeedforward = feedforward; } + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Velocity Feed Forward Controller"); + builder.addDoubleProperty("Setpoint", this::getSetpoint, null); + builder.addDoubleProperty("Measurement", this::getMeasurement, null); + builder.addDoubleProperty("Output", this::getOutput, null); + builder.addDoubleProperty("Error", this::getError, null); + } + /** * Calculates a motor output by feeding a velocity setpoint to a feedforward model. * From ab17e406ccfdd6d51b84ecda6401154b93847249 Mon Sep 17 00:00:00 2001 From: Vincent Wang Date: Fri, 31 Mar 2023 13:23:33 -0400 Subject: [PATCH 2/3] Make Angle Controller Sendable --- .../control/angle/AngleController.java | 15 +++++++++++- .../control/angle/AngleControllerGroup.java | 24 +++++++++++++++++-- .../angle/feedback/AnglePIDCalculator.java | 11 +++++++++ .../angle/feedback/AnglePIDController.java | 11 +++++++++ .../feedforward/AngleArmFeedforward.java | 11 +++++++++ .../AnglePositionFeedforwardController.java | 11 +++++++++ 6 files changed, 80 insertions(+), 3 deletions(-) diff --git a/src/com/stuypulse/stuylib/control/angle/AngleController.java b/src/com/stuypulse/stuylib/control/angle/AngleController.java index 0abd1bea..485cb682 100644 --- a/src/com/stuypulse/stuylib/control/angle/AngleController.java +++ b/src/com/stuypulse/stuylib/control/angle/AngleController.java @@ -8,6 +8,10 @@ import com.stuypulse.stuylib.streams.angles.filters.AFilter; import com.stuypulse.stuylib.streams.filters.IFilter; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl; + /** * Base class of controller classes of continuous systems. This means that both the setpoint and * measurement are angles, as opposed to just numbers. @@ -18,7 +22,7 @@ * @see com.stuypulse.stuylib.control.Controller * @author Myles Pasetsky (myles.pasetsky@gmail.com) */ -public abstract class AngleController { +public abstract class AngleController implements Sendable { /** The most recent setpoint of the controller */ private Angle mSetpoint; @@ -49,6 +53,15 @@ public AngleController() { mOutputFilter = x -> x; } + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Angle Controller"); + builder.addDoubleProperty("Setpoint", () -> getSetpoint().toDegrees(), null); + builder.addDoubleProperty("Measurement", () -> getMeasurement().toDegrees(), null); + builder.addDoubleProperty("Output", this::getOutput, null); + builder.addDoubleProperty("Error", () -> getError().toDegrees(), null); + } + /** * Set the setpoint filter of the controller * diff --git a/src/com/stuypulse/stuylib/control/angle/AngleControllerGroup.java b/src/com/stuypulse/stuylib/control/angle/AngleControllerGroup.java index 4448d0da..fe396b70 100644 --- a/src/com/stuypulse/stuylib/control/angle/AngleControllerGroup.java +++ b/src/com/stuypulse/stuylib/control/angle/AngleControllerGroup.java @@ -4,8 +4,12 @@ package com.stuypulse.stuylib.control.angle; +import java.util.ArrayList; + import com.stuypulse.stuylib.math.Angle; +import edu.wpi.first.util.sendable.SendableBuilder; + /** * Angle controllers can be grouped together in a "controller group" if they have the same setpoint * and measurement. @@ -21,12 +25,28 @@ public class AngleControllerGroup extends AngleController { private final AngleController mController; /** Controllers part of the group */ - private final AngleController[] mControllers; + private final ArrayList mControllers; /** Create a controller group */ public AngleControllerGroup(AngleController controller, AngleController... controllers) { mController = controller; - mControllers = controllers; + + mControllers = new ArrayList<>(); + + for (AngleController tmpController : controllers) { + if (controller == null) { + throw new IllegalArgumentException("Controller cannot be null"); + } + mControllers.add(tmpController); + } + } + + @Override + public void initSendable(SendableBuilder builder) { + mController.initSendable(builder); + for (AngleController controller : mControllers) { + controller.initSendable(builder); + } } /** diff --git a/src/com/stuypulse/stuylib/control/angle/feedback/AnglePIDCalculator.java b/src/com/stuypulse/stuylib/control/angle/feedback/AnglePIDCalculator.java index 87250624..f612a24e 100644 --- a/src/com/stuypulse/stuylib/control/angle/feedback/AnglePIDCalculator.java +++ b/src/com/stuypulse/stuylib/control/angle/feedback/AnglePIDCalculator.java @@ -13,6 +13,8 @@ import com.stuypulse.stuylib.streams.filters.TimedMovingAverage; import com.stuypulse.stuylib.util.StopWatch; +import edu.wpi.first.util.sendable.SendableBuilder; + /** * This is a Bang-Bang controller that while controlling the robot, will be able to calculate the * values for the PID controller. It does this by taking the results of oscillations, then creating @@ -79,6 +81,15 @@ public AnglePIDCalculator(Number speed) { mRunning = false; } + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Angle PID Calculator"); + builder.addDoubleProperty("Setpoint", () -> getSetpoint().toDegrees(), null); + builder.addDoubleProperty("Measurement", () -> getMeasurement().toDegrees(), null); + builder.addDoubleProperty("Output", this::getOutput, null); + builder.addDoubleProperty("Error", () -> getError().toDegrees(), null); + } + /** * @param speed sets speed for motor output of controller * @return the calculated result from the PIDController diff --git a/src/com/stuypulse/stuylib/control/angle/feedback/AnglePIDController.java b/src/com/stuypulse/stuylib/control/angle/feedback/AnglePIDController.java index 41c78bf5..365acd37 100644 --- a/src/com/stuypulse/stuylib/control/angle/feedback/AnglePIDController.java +++ b/src/com/stuypulse/stuylib/control/angle/feedback/AnglePIDController.java @@ -12,6 +12,8 @@ import com.stuypulse.stuylib.streams.filters.IFilterGroup; import com.stuypulse.stuylib.util.StopWatch; +import edu.wpi.first.util.sendable.SendableBuilder; + /** * This PID controller is built by extending the AngleController class. It has a dynamic rate, so it * can detect how much time has passed between each update. It is made to be easy to use and simple @@ -60,6 +62,15 @@ public AnglePIDController(Number p, Number i, Number d) { reset(); } + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Angle PID Controller"); + builder.addDoubleProperty("Setpoint", () -> getSetpoint().toDegrees(), null); + builder.addDoubleProperty("Measurement", () -> getMeasurement().toDegrees(), null); + builder.addDoubleProperty("Output", this::getOutput, null); + builder.addDoubleProperty("Error", () -> getError().toDegrees(), null); + } + /** Creates a blank PIDController that doesn't move */ public AnglePIDController() { this(-1, -1, -1); diff --git a/src/com/stuypulse/stuylib/control/angle/feedforward/AngleArmFeedforward.java b/src/com/stuypulse/stuylib/control/angle/feedforward/AngleArmFeedforward.java index 10d6af00..b3069339 100644 --- a/src/com/stuypulse/stuylib/control/angle/feedforward/AngleArmFeedforward.java +++ b/src/com/stuypulse/stuylib/control/angle/feedforward/AngleArmFeedforward.java @@ -7,6 +7,8 @@ import com.stuypulse.stuylib.control.angle.AngleController; import com.stuypulse.stuylib.math.Angle; +import edu.wpi.first.util.sendable.SendableBuilder; + /** * A feedforward term to account for gravity for motorized arms that can move continuously (if not * use `ArmFeedforward`) @@ -32,6 +34,15 @@ public AngleArmFeedforward(Number kG) { this.kG = kG; } + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Angle Arm Feed Forward"); + builder.addDoubleProperty("Setpoint", () -> getSetpoint().toDegrees(), null); + builder.addDoubleProperty("Measurement", () -> getMeasurement().toDegrees(), null); + builder.addDoubleProperty("Output", this::getOutput, null); + builder.addDoubleProperty("Error", () -> getError().toDegrees(), null); + } + /** * Calculates voltage to hold arm at the setpoint angle * diff --git a/src/com/stuypulse/stuylib/control/angle/feedforward/AnglePositionFeedforwardController.java b/src/com/stuypulse/stuylib/control/angle/feedforward/AnglePositionFeedforwardController.java index 0e83b94d..8f277b8b 100644 --- a/src/com/stuypulse/stuylib/control/angle/feedforward/AnglePositionFeedforwardController.java +++ b/src/com/stuypulse/stuylib/control/angle/feedforward/AnglePositionFeedforwardController.java @@ -9,6 +9,8 @@ import com.stuypulse.stuylib.math.Angle; import com.stuypulse.stuylib.util.AngleVelocity; +import edu.wpi.first.util.sendable.SendableBuilder; + /** * A positional feedforward controller for angular systems. * @@ -33,6 +35,15 @@ public AnglePositionFeedforwardController(MotorFeedforward feedforward) { mDerivative = new AngleVelocity(); } + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Angle Position Feed Forward Controller"); + builder.addDoubleProperty("Setpoint", () -> getSetpoint().toDegrees(), null); + builder.addDoubleProperty("Measurement", () -> getMeasurement().toDegrees(), null); + builder.addDoubleProperty("Output", this::getOutput, null); + builder.addDoubleProperty("Error", () -> getError().toDegrees(), null); + } + /** * Calculates a motor output by feeding the derivative of a positional setpoint to a feedforward * model From 6d67b661cd3da64e8cf0742c5ee8de5e6da85242 Mon Sep 17 00:00:00 2001 From: Vincent Wang Date: Fri, 31 Mar 2023 14:13:03 -0400 Subject: [PATCH 3/3] Change names of data for controllers --- .../stuypulse/stuylib/control/ControllerGroup.java | 11 ++--------- .../stuylib/control/angle/AngleController.java | 8 ++++---- .../stuylib/control/angle/AngleControllerGroup.java | 12 ++++-------- .../control/angle/feedback/AnglePIDCalculator.java | 8 ++++---- .../control/angle/feedback/AnglePIDController.java | 8 ++++---- .../angle/feedforward/AngleArmFeedforward.java | 8 ++++---- .../AnglePositionFeedforwardController.java | 8 ++++---- .../stuylib/control/feedback/PIDCalculator.java | 8 ++++---- .../stuylib/control/feedback/PIDController.java | 8 ++++---- .../stuylib/control/feedforward/ArmFeedforward.java | 8 ++++---- .../control/feedforward/ElevatorFeedforward.java | 8 ++++---- .../feedforward/PositionFeedforwardController.java | 8 ++++---- .../feedforward/VelocityFeedforwardController.java | 8 ++++---- 13 files changed, 50 insertions(+), 61 deletions(-) diff --git a/src/com/stuypulse/stuylib/control/ControllerGroup.java b/src/com/stuypulse/stuylib/control/ControllerGroup.java index 23dc3005..c21ecc4c 100644 --- a/src/com/stuypulse/stuylib/control/ControllerGroup.java +++ b/src/com/stuypulse/stuylib/control/ControllerGroup.java @@ -20,28 +20,21 @@ */ public class ControllerGroup extends Controller { - /** Controller part of the group */ - private final Controller mController; - /** Controllers part of the group */ private final ArrayList mControllers; /** Create a controller group */ public ControllerGroup(Controller controller, Controller... controllers) { - mController = controller; mControllers = new ArrayList<>(); + mControllers.add(controller); for (Controller tmpController : controllers) { - if (controller == null) { - throw new IllegalArgumentException("Controller cannot be null"); - } mControllers.add(tmpController); } } @Override public void initSendable(SendableBuilder builder) { - mController.initSendable(builder); for (Controller controller : mControllers) { controller.initSendable(builder); } @@ -68,7 +61,7 @@ public ControllerGroup add(Controller... controller) { */ @Override protected double calculate(double setpoint, double measurement) { - double output = mController.update(setpoint, measurement); + double output = 0; for (Controller controller : mControllers) { output += controller.update(setpoint, measurement); diff --git a/src/com/stuypulse/stuylib/control/angle/AngleController.java b/src/com/stuypulse/stuylib/control/angle/AngleController.java index 485cb682..707358e2 100644 --- a/src/com/stuypulse/stuylib/control/angle/AngleController.java +++ b/src/com/stuypulse/stuylib/control/angle/AngleController.java @@ -56,10 +56,10 @@ public AngleController() { @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("Angle Controller"); - builder.addDoubleProperty("Setpoint", () -> getSetpoint().toDegrees(), null); - builder.addDoubleProperty("Measurement", () -> getMeasurement().toDegrees(), null); - builder.addDoubleProperty("Output", this::getOutput, null); - builder.addDoubleProperty("Error", () -> getError().toDegrees(), null); + builder.addDoubleProperty("Angle Controller Setpoint", () -> getSetpoint().toDegrees(), null); + builder.addDoubleProperty("Angle Controller Measurement", () -> getMeasurement().toDegrees(), null); + builder.addDoubleProperty("Angle Controller Output", this::getOutput, null); + builder.addDoubleProperty("Angle Controller Error", () -> getError().toDegrees(), null); } /** diff --git a/src/com/stuypulse/stuylib/control/angle/AngleControllerGroup.java b/src/com/stuypulse/stuylib/control/angle/AngleControllerGroup.java index fe396b70..6a52d1cb 100644 --- a/src/com/stuypulse/stuylib/control/angle/AngleControllerGroup.java +++ b/src/com/stuypulse/stuylib/control/angle/AngleControllerGroup.java @@ -21,20 +21,17 @@ */ public class AngleControllerGroup extends AngleController { - /** Controller part of the group */ - private final AngleController mController; - /** Controllers part of the group */ private final ArrayList mControllers; /** Create a controller group */ public AngleControllerGroup(AngleController controller, AngleController... controllers) { - mController = controller; - mControllers = new ArrayList<>(); + mControllers = new ArrayList<>(controllers.length); + mControllers.add(controller); for (AngleController tmpController : controllers) { - if (controller == null) { + if (tmpController == null) { throw new IllegalArgumentException("Controller cannot be null"); } mControllers.add(tmpController); @@ -43,7 +40,6 @@ public AngleControllerGroup(AngleController controller, AngleController... contr @Override public void initSendable(SendableBuilder builder) { - mController.initSendable(builder); for (AngleController controller : mControllers) { controller.initSendable(builder); } @@ -59,7 +55,7 @@ public void initSendable(SendableBuilder builder) { */ @Override protected double calculate(Angle setpoint, Angle measurement) { - double output = mController.update(setpoint, measurement); + double output = 0; for (AngleController controller : mControllers) { output += controller.update(setpoint, measurement); diff --git a/src/com/stuypulse/stuylib/control/angle/feedback/AnglePIDCalculator.java b/src/com/stuypulse/stuylib/control/angle/feedback/AnglePIDCalculator.java index f612a24e..97672a01 100644 --- a/src/com/stuypulse/stuylib/control/angle/feedback/AnglePIDCalculator.java +++ b/src/com/stuypulse/stuylib/control/angle/feedback/AnglePIDCalculator.java @@ -84,10 +84,10 @@ public AnglePIDCalculator(Number speed) { @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("Angle PID Calculator"); - builder.addDoubleProperty("Setpoint", () -> getSetpoint().toDegrees(), null); - builder.addDoubleProperty("Measurement", () -> getMeasurement().toDegrees(), null); - builder.addDoubleProperty("Output", this::getOutput, null); - builder.addDoubleProperty("Error", () -> getError().toDegrees(), null); + builder.addDoubleProperty("PID Calculator Setpoint", () -> getSetpoint().toDegrees(), null); + builder.addDoubleProperty("PID Calculator Measurement", () -> getMeasurement().toDegrees(), null); + builder.addDoubleProperty("PID Calculator Output", this::getOutput, null); + builder.addDoubleProperty("PID Calculator Error", () -> getError().toDegrees(), null); } /** diff --git a/src/com/stuypulse/stuylib/control/angle/feedback/AnglePIDController.java b/src/com/stuypulse/stuylib/control/angle/feedback/AnglePIDController.java index 365acd37..6f3440a3 100644 --- a/src/com/stuypulse/stuylib/control/angle/feedback/AnglePIDController.java +++ b/src/com/stuypulse/stuylib/control/angle/feedback/AnglePIDController.java @@ -65,10 +65,10 @@ public AnglePIDController(Number p, Number i, Number d) { @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("Angle PID Controller"); - builder.addDoubleProperty("Setpoint", () -> getSetpoint().toDegrees(), null); - builder.addDoubleProperty("Measurement", () -> getMeasurement().toDegrees(), null); - builder.addDoubleProperty("Output", this::getOutput, null); - builder.addDoubleProperty("Error", () -> getError().toDegrees(), null); + builder.addDoubleProperty("PID Setpoint", () -> getSetpoint().toDegrees(), null); + builder.addDoubleProperty("PID Measurement", () -> getMeasurement().toDegrees(), null); + builder.addDoubleProperty("PID Output", this::getOutput, null); + builder.addDoubleProperty("PID Error", () -> getError().toDegrees(), null); } /** Creates a blank PIDController that doesn't move */ diff --git a/src/com/stuypulse/stuylib/control/angle/feedforward/AngleArmFeedforward.java b/src/com/stuypulse/stuylib/control/angle/feedforward/AngleArmFeedforward.java index b3069339..3c82e274 100644 --- a/src/com/stuypulse/stuylib/control/angle/feedforward/AngleArmFeedforward.java +++ b/src/com/stuypulse/stuylib/control/angle/feedforward/AngleArmFeedforward.java @@ -37,10 +37,10 @@ public AngleArmFeedforward(Number kG) { @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("Angle Arm Feed Forward"); - builder.addDoubleProperty("Setpoint", () -> getSetpoint().toDegrees(), null); - builder.addDoubleProperty("Measurement", () -> getMeasurement().toDegrees(), null); - builder.addDoubleProperty("Output", this::getOutput, null); - builder.addDoubleProperty("Error", () -> getError().toDegrees(), null); + builder.addDoubleProperty("Angle Arm FF Setpoint", () -> getSetpoint().toDegrees(), null); + builder.addDoubleProperty("Angle Arm FF Measurement", () -> getMeasurement().toDegrees(), null); + builder.addDoubleProperty("Angle Arm FF Output", this::getOutput, null); + builder.addDoubleProperty("Angle Arm FF Error", () -> getError().toDegrees(), null); } /** diff --git a/src/com/stuypulse/stuylib/control/angle/feedforward/AnglePositionFeedforwardController.java b/src/com/stuypulse/stuylib/control/angle/feedforward/AnglePositionFeedforwardController.java index 8f277b8b..03a58119 100644 --- a/src/com/stuypulse/stuylib/control/angle/feedforward/AnglePositionFeedforwardController.java +++ b/src/com/stuypulse/stuylib/control/angle/feedforward/AnglePositionFeedforwardController.java @@ -38,10 +38,10 @@ public AnglePositionFeedforwardController(MotorFeedforward feedforward) { @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("Angle Position Feed Forward Controller"); - builder.addDoubleProperty("Setpoint", () -> getSetpoint().toDegrees(), null); - builder.addDoubleProperty("Measurement", () -> getMeasurement().toDegrees(), null); - builder.addDoubleProperty("Output", this::getOutput, null); - builder.addDoubleProperty("Error", () -> getError().toDegrees(), null); + builder.addDoubleProperty("Angle Position FF Setpoint", () -> getSetpoint().toDegrees(), null); + builder.addDoubleProperty("Angle Position FF Measurement", () -> getMeasurement().toDegrees(), null); + builder.addDoubleProperty("Angle Position FF Output", this::getOutput, null); + builder.addDoubleProperty("Angle Position FF Error", () -> getError().toDegrees(), null); } /** diff --git a/src/com/stuypulse/stuylib/control/feedback/PIDCalculator.java b/src/com/stuypulse/stuylib/control/feedback/PIDCalculator.java index 1d8521d5..b2b21265 100644 --- a/src/com/stuypulse/stuylib/control/feedback/PIDCalculator.java +++ b/src/com/stuypulse/stuylib/control/feedback/PIDCalculator.java @@ -83,10 +83,10 @@ public PIDCalculator(Number speed) { @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("PID Calculator"); - builder.addDoubleProperty("Setpoint", this::getSetpoint, null); - builder.addDoubleProperty("Measurement", this::getMeasurement, null); - builder.addDoubleProperty("Output", this::getOutput, null); - builder.addDoubleProperty("Error", this::getError, null); + builder.addDoubleProperty("PID Calculator Setpoint", this::getSetpoint, null); + builder.addDoubleProperty("PID Calculator Measurement", this::getMeasurement, null); + builder.addDoubleProperty("PID Calculator Output", this::getOutput, null); + builder.addDoubleProperty("PID Calculator Error", this::getError, null); } /** diff --git a/src/com/stuypulse/stuylib/control/feedback/PIDController.java b/src/com/stuypulse/stuylib/control/feedback/PIDController.java index 4fc8f70b..4ca36fd3 100644 --- a/src/com/stuypulse/stuylib/control/feedback/PIDController.java +++ b/src/com/stuypulse/stuylib/control/feedback/PIDController.java @@ -71,10 +71,10 @@ public PIDController(Number p, Number i, Number d) { @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("PID Controller"); - builder.addDoubleProperty("Setpoint", this::getSetpoint, null); - builder.addDoubleProperty("Measurement", this::getMeasurement, null); - builder.addDoubleProperty("Output", this::getOutput, null); - builder.addDoubleProperty("Error", this::getError, null); + builder.addDoubleProperty("PID Setpoint", this::getSetpoint, null); + builder.addDoubleProperty("PID Measurement", this::getMeasurement, null); + builder.addDoubleProperty("PID Output", this::getOutput, null); + builder.addDoubleProperty("PID Error", this::getError, null); } /** Creates a blank PIDController that doesn't move */ diff --git a/src/com/stuypulse/stuylib/control/feedforward/ArmFeedforward.java b/src/com/stuypulse/stuylib/control/feedforward/ArmFeedforward.java index 5af065d9..c1aaa80b 100644 --- a/src/com/stuypulse/stuylib/control/feedforward/ArmFeedforward.java +++ b/src/com/stuypulse/stuylib/control/feedforward/ArmFeedforward.java @@ -52,10 +52,10 @@ public ArmFeedforward(Number kG, IFilter cosine) { @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("Arm Feed Forward"); - builder.addDoubleProperty("Setpoint", this::getSetpoint, null); - builder.addDoubleProperty("Measurement", this::getMeasurement, null); - builder.addDoubleProperty("Output", this::getOutput, null); - builder.addDoubleProperty("Error", this::getError, null); + builder.addDoubleProperty("Arm FF Setpoint", this::getSetpoint, null); + builder.addDoubleProperty("Arm FF Measurement", this::getMeasurement, null); + builder.addDoubleProperty("Arm FF Output", this::getOutput, null); + builder.addDoubleProperty("Arm FF Error", this::getError, null); } /** diff --git a/src/com/stuypulse/stuylib/control/feedforward/ElevatorFeedforward.java b/src/com/stuypulse/stuylib/control/feedforward/ElevatorFeedforward.java index 9ca5a884..b0c3e519 100644 --- a/src/com/stuypulse/stuylib/control/feedforward/ElevatorFeedforward.java +++ b/src/com/stuypulse/stuylib/control/feedforward/ElevatorFeedforward.java @@ -35,10 +35,10 @@ public ElevatorFeedforward(Number kG) { @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("Elevator Feed Forward"); - builder.addDoubleProperty("Setpoint", this::getSetpoint, null); - builder.addDoubleProperty("Measurement", this::getMeasurement, null); - builder.addDoubleProperty("Output", this::getOutput, null); - builder.addDoubleProperty("Error", this::getError, null); + builder.addDoubleProperty("Elevator FF Setpoint", this::getSetpoint, null); + builder.addDoubleProperty("Elevator FF Measurement", this::getMeasurement, null); + builder.addDoubleProperty("Elevator FF Output", this::getOutput, null); + builder.addDoubleProperty("Elevator FF Error", this::getError, null); } /** diff --git a/src/com/stuypulse/stuylib/control/feedforward/PositionFeedforwardController.java b/src/com/stuypulse/stuylib/control/feedforward/PositionFeedforwardController.java index 6146ed7b..e6dcf2aa 100644 --- a/src/com/stuypulse/stuylib/control/feedforward/PositionFeedforwardController.java +++ b/src/com/stuypulse/stuylib/control/feedforward/PositionFeedforwardController.java @@ -39,10 +39,10 @@ public PositionFeedforwardController(MotorFeedforward feedforward) { @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("Position Feed Forward Controller"); - builder.addDoubleProperty("Setpoint", this::getSetpoint, null); - builder.addDoubleProperty("Measurement", this::getMeasurement, null); - builder.addDoubleProperty("Output", this::getOutput, null); - builder.addDoubleProperty("Error", this::getError, null); + builder.addDoubleProperty("Position FF Setpoint", this::getSetpoint, null); + builder.addDoubleProperty("Position FF Measurement", this::getMeasurement, null); + builder.addDoubleProperty("Position FF Output", this::getOutput, null); + builder.addDoubleProperty("Position FF Error", this::getError, null); } /** diff --git a/src/com/stuypulse/stuylib/control/feedforward/VelocityFeedforwardController.java b/src/com/stuypulse/stuylib/control/feedforward/VelocityFeedforwardController.java index 345ee999..7619de2d 100644 --- a/src/com/stuypulse/stuylib/control/feedforward/VelocityFeedforwardController.java +++ b/src/com/stuypulse/stuylib/control/feedforward/VelocityFeedforwardController.java @@ -31,10 +31,10 @@ public VelocityFeedforwardController(MotorFeedforward feedforward) { @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("Velocity Feed Forward Controller"); - builder.addDoubleProperty("Setpoint", this::getSetpoint, null); - builder.addDoubleProperty("Measurement", this::getMeasurement, null); - builder.addDoubleProperty("Output", this::getOutput, null); - builder.addDoubleProperty("Error", this::getError, null); + builder.addDoubleProperty("Velocity FF Setpoint", this::getSetpoint, null); + builder.addDoubleProperty("Velocity FF Measurement", this::getMeasurement, null); + builder.addDoubleProperty("Velocity FF Output", this::getOutput, null); + builder.addDoubleProperty("Velocity FF Error", this::getError, null); } /**