From 8b64749fd5073c5ebbd8c26fdc4307586a2e9d0c Mon Sep 17 00:00:00 2001 From: Mason-Lam <97353903+Mason-Lam@users.noreply.github.com> Date: Fri, 24 Nov 2023 16:58:11 -0800 Subject: [PATCH] Controller Refactoring Removed PController and VController, combined functionality in Controller class --- .../common/core/controllers/Controller.java | 178 ++++++++++++------ .../common/core/controllers/PController.java | 63 ------- .../common/core/controllers/VController.java | 61 ------ .../core/subsystems/NAR_PIDSubsystem.java | 19 +- 4 files changed, 133 insertions(+), 188 deletions(-) delete mode 100644 src/main/java/common/core/controllers/PController.java delete mode 100644 src/main/java/common/core/controllers/VController.java diff --git a/src/main/java/common/core/controllers/Controller.java b/src/main/java/common/core/controllers/Controller.java index 6169d32..57b2bd2 100644 --- a/src/main/java/common/core/controllers/Controller.java +++ b/src/main/java/common/core/controllers/Controller.java @@ -4,9 +4,9 @@ import java.util.function.DoubleConsumer; import java.util.function.DoubleSupplier; -import common.core.subsystems.NAR_PIDSubsystem; import common.hardware.motorcontroller.NAR_Motor; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; /** * Team 3128's streamlined {@link PIDController} class. @@ -16,80 +16,139 @@ */ public class Controller extends PIDController { + /** + * Setpoint types + */ + public enum Type { + VELOCITY, + POSITION, + DEFAULT; + } + private final LinkedList consumers; + private final Type type; private DoubleSupplier measurement; protected DoubleSupplier kS, kV, kG; protected DoubleSupplier kG_Function; + + /** + * Create a new object to control PID + FF logic for a subsystem. + *

Sets kP, kI, kD, kS, kV, kG, period values. + * + * @param kP The proportional coefficient. + * @param kI The integral coefficient. + * @param kD The derivative coefficient. + * @param kS The static gain. + * @param kV The velocity gain. + * @param kG The gravity gain. + * @param type The type of setpoint used by the controller + * @param period The controller's update rate in seconds. Must be non-zero and positive. + */ + public Controller(double kP, double kI, double kD, double kS, double kV, double kG, Type type, double period) { + super(kP, kI, kD, period); + consumers = new LinkedList(); + this.kS = ()-> kS; + this.kV = ()-> kV; + this.kG = ()-> kG; + kG_Function = ()-> 1; + this.type = type; + } + /** * Create a new object to control PID logic for a subsystem. *

Sets kP, kI, kD, period values. - *

Defaults kS, kV, kG to 0. - *

Defaults kG_Function to 1. - *

Initiliazes consumers. - * @see LinkedList consumers: includes void functions that accept double values. * - * @param kP The proportional coefficient of the on board PIDController. - * @param kI The integral coefficient of the on board PIDController. - * @param kD The derivative coefficient of the on board PIDController. - * @param period The PID periodic update rate in seconds. + * @param kP The proportional coefficient. + * @param kI The integral coefficient. + * @param kD The derivative coefficient. + * @param period The controller's update rate in seconds. Must be non-zero and positive. */ public Controller(double kP, double kI, double kD, double period) { - super(kP, kI, kD, period); - kS = ()-> 0; - kV = ()-> 0; - kG = ()-> 0; - kG_Function = () -> 1; - consumers = new LinkedList(); + this(kP, kI, kD, 0, 0, 0, Type.DEFAULT, period); } /** * Create a new object to control PID logic for a subsystem. *

Sets kP, kI, kD values. - *

Defaults period to 0.02. - *

Defaults kS, kV, kG to 0. - *

Defaults kG_Function to 1. - *

Initiliazes consumers. - * @see LinkedList consumers: includes void functions that accept double values. * - * @param kP The proportional coefficient of the on board {@link PIDController}. - * @param kI The integral coefficient of the on board {@link PIDController}. - * @param kD The derivative coefficient of the on board {@link PIDController}. + * @param kP The proportional coefficient. + * @param kI The integral coefficient. + * @param kD The derivative coefficient. */ public Controller(double kP, double kI, double kD) { this(kP, kI, kD, 0.02); } /** - * Sets the measurement source to be later used during PID logic. - * @param measurement {@code getMeasurement()} from {@link NAR_PIDSubsystem} as a DoubleSupplier + * Sets a motor's velocity/position as the measurement source + * @param motor {@link NAR_Motor} motor type. + */ + public void setMotorMeasurementSource(NAR_Motor motor) { + if (type == Type.VELOCITY) { + setMeasurementSource(()-> motor.getVelocity()); + return; + } + setMeasurementSource(()-> motor.getPosition()); + } + + /** + * Sets the measurement source for PID control. + * @param measurement Returns the current state of the system. */ public void setMeasurementSource(DoubleSupplier measurement) { this.measurement = measurement; } /** - * Adds a motor set method to consumers to be later used during PID logic. - * @see LinkedList consumers: includes void functions that accept double values. - * @param motor Created {@link NAR_Motor} object. + * Adds a motor for controller to send output + * @param motor {@link NAR_Motor} motor type. */ public void addMotor(NAR_Motor motor) { - consumers.add(motor::set); + addOutput(motor::set); + } + + /** + * Adds a motor for controller to send output + * @param motor {@link MotorController} motor type. + */ + public void addMotor(MotorController motor) { + addOutput(motor::set); } + /** - * Adds a useOutput method to consumers to be later used during PID logic. - * @see LinkedList consumers: includes void functions that accept double values. - * @param output {@code useOutput()} from {@link NAR_PIDSubsystem} as a DoubleSupplier + * Adds an output source for controller to send output + * @param output Method that takes the controller's output */ public void addOutput(DoubleConsumer output) { consumers.add(output); } /** - * Calculates the output based on calculations from the measurement, then gives each DoubleConsumer in consumers the output as an argument. - * @return calculated output based on calculations from the measurement - * @see LinkedList consumers: includes void functions that accept double values. + * Returns the next output of the controller. + * + * @param measurement The current measurement of the process variable. + * @return The next controller output. + */ + @Override + public double calculate(double measurement) { + final double PID_OUTPUT = super.calculate(measurement); + if (type == Type.DEFAULT) return PID_OUTPUT; + + double ff_output = Math.copySign(getkS(), PID_OUTPUT); + + if (type == Type.VELOCITY) { + ff_output += getkV() * getSetpoint(); + return ff_output + PID_OUTPUT; + } + ff_output += getkG() * kG_Function.getAsDouble(); + return ff_output + PID_OUTPUT; + } + + /** + * Calculates and sends output. + * @return Calculated output. */ public double useOutput() { if (measurement == null) return 0; @@ -101,15 +160,7 @@ public double useOutput() { } /** - * Sets the kG_Function to a new DoubleSupplier. - * @param kG_Function DoubleSupplier with specified logic. - */ - public void setkG_Function(DoubleSupplier kG_Function) { - this.kG_Function = kG_Function; - } - - /** - * Sets the kS value. + * Sets the static gain. * @param kS The constant power required to overcome static friction as a double. */ public void setkS(double kS) { @@ -117,15 +168,15 @@ public void setkS(double kS) { } /** - * Sets the kS value. - * @param kS The constant power required to overcome static friction as a DoubleSupplier. + * Sets the static gain. + * @param kS Modifies kS based on the function. */ public void setkS(DoubleSupplier kS) { this.kS = kS; } /** - * Sets the kV value. + * Sets the velocity gain. * @param kV The constant power required to maintain a set velocity as a double. */ public void setkV(double kV) { @@ -133,15 +184,15 @@ public void setkV(double kV) { } /** - * Sets the kV value. - * @param kV The constant power required to maintain a set velocity as a DoubleSupplier. + * Sets the velocity gain. + * @param kV Modifies kV based on the function. */ public void setkV(DoubleSupplier kV) { this.kV = kV; } /** - * Sets the kG value. + * Sets the gravity gain. * @param kG The constant power required to overcome gravity as a double. */ public void setkG(double kG) { @@ -149,15 +200,32 @@ public void setkG(double kG) { } /** - * Sets the kG value. - * @param kG The constant power required to overcome gravity as a DoubleSupplier. + * Sets the gravity gain. + * @param kG Modifies kG based on the function. */ public void setkG(DoubleSupplier kG) { this.kG = kG; } /** - * Returns kS. + * Sets a function to modify gravity gain based on another factor. + *

Example use would be a Pivot which would have gravity gain dependent on angle. + * @param kG_Function DoubleSupplier with specified logic. + */ + public void setkG_Function(DoubleSupplier kG_Function) { + this.kG_Function = kG_Function; + } + + /** + * Returns the type of setpoint used by the controller + * @return {@link Type}: VELOCITY, POSITION, NONE + */ + public Type getType() { + return type; + } + + /** + * Returns static gain. * @return returns kS as a double. */ public double getkS() { @@ -165,7 +233,7 @@ public double getkS() { } /** - * Returns kV. + * Returns velocity gain. * @return returns kV as a double. */ public double getkV() { @@ -173,7 +241,7 @@ public double getkV() { } /** - * Returns kG. + * Returns gravity gain. * @return returns kG as a double. */ public double getkG() { diff --git a/src/main/java/common/core/controllers/PController.java b/src/main/java/common/core/controllers/PController.java deleted file mode 100644 index 8d392ab..0000000 --- a/src/main/java/common/core/controllers/PController.java +++ /dev/null @@ -1,63 +0,0 @@ -package common.core.controllers; -import edu.wpi.first.math.controller.PIDController; - -/** - * Team 3128's Position Controller class extending the {@link Controller} class. - * - * @since 2023 CHARGED UP - * @author Mason Lam, William Yuan - */ -public class PController extends Controller { - /** - * Create a new object to control position PID logic for a subsystem. - *

Sets kP, kI, kD, period values. - *

Sets kS, kG values. - *

Defaults kG_Function to 1. - * - * @param kS The constant power required to overcome static friction. - * @param kG The constant power required to overcome gravity. - * @param kP The proportional coefficient of the on board PIDController. - * @param kI The integral coefficient of the on board PIDController. - * @param kD The derivative coefficient of the on board PIDController. - * @param period The PID periodic update rate in seconds. - */ - public PController(double kS, double kG, double kP, double kI, double kD, double period) { - super(kP, kI, kD, period); - setkS(()-> kS); - setkG(()-> kG); - setkG_Function(() -> 1); - } - - /** - * Create a new object to control position PID logic for a subsystem. - *

Sets kP, kI, kD values. - *

Sets kS, kG values. - *

Defaults kG_Function to 1. - *

Defaults period to 0.02. - * - * @param kS The constant power required to overcome static friction. - * @param kG The constant power required to overcome gravity. - * @param kP The proportional coefficient of the on board PIDController. - * @param kI The integral coefficient of the on board PIDController. - * @param kD The derivative coefficient of the on board PIDController. - */ - public PController(double kS, double kG, double kP, double kI, double kD) { - this(kS, kG, kP, kI, kD, 0.02); - } - - /** - * Overrides the calculate method in {@link PIDController} to use PID and FF logic. - *

Uses kG instead of kV. - * - * @param measurement The desired setpoint for the subsystem. - * - * @return the output from PID and FF. - */ - @Override - public double calculate(double measurement) { - final double pid = super.calculate(measurement); - final double ff = !atSetpoint() ? Math.copySign(getkS(), pid) : 0 + getkG() * kG_Function.getAsDouble(); - return pid + ff; - } - -} diff --git a/src/main/java/common/core/controllers/VController.java b/src/main/java/common/core/controllers/VController.java deleted file mode 100644 index 7f96084..0000000 --- a/src/main/java/common/core/controllers/VController.java +++ /dev/null @@ -1,61 +0,0 @@ -package common.core.controllers; - -import edu.wpi.first.math.controller.PIDController; - -/** - * Team 3128's Velocity Controller class extending the {@link Controller} class. - * - * @since 2023 CHARGED UP - * @author Mason Lam, William Yuan - */ -public class VController extends Controller { - /** - * Create a new object to control velocity PID logic for a subsystem. - *

Sets kP, kI, kD, period values. - *

Sets kS, kV values. - * - * @param kS The constant power required to overcome static friction. - * @param kV The constant power required to maintain a set velocity. - * @param kP The proportional coefficient of the on board PIDController. - * @param kI The integral coefficient of the on board PIDController. - * @param kD The derivative coefficient of the on board PIDController. - * @param period The PID periodic update rate in seconds. - */ - public VController(double kS, double kV, double kP, double kI, double kD, double period) { - super(kP, kI, kD, period); - setkS(()-> kS); - setkV(()-> kV); - } - - /** - * Create a new object to control velocity PID logic for a subsystem. - *

Sets kP, kI, kD values. - *

Sets kS, kV values. - *

Defaults period to 0.02. - * - * @param kS The constant power required to overcome static friction. - * @param kV The constant power required to maintain a set velocity. - * @param kP The proportional coefficient of the on board PIDController. - * @param kI The integral coefficient of the on board PIDController. - * @param kD The derivative coefficient of the on board PIDController. - */ - public VController(double kS, double kV, double kP, double kI, double kD) { - this(kS, kV, kP, kI, kD, 0.02); - } - - /** - * Overrides the calculate method in {@link PIDController} to use PID and FF logic. - *

Uses kV instead of kG. - * - * @param measurement The desired setpoint for the subsystem. - * - * @return the output from PID and FF. - */ - @Override - public double calculate(double measurement) { - final double pid = super.calculate(measurement); - final double ff = !atSetpoint() ? Math.copySign(getkS(), pid) : 0 + getkV() * getSetpoint(); - return pid + ff; - } - -} diff --git a/src/main/java/common/core/subsystems/NAR_PIDSubsystem.java b/src/main/java/common/core/subsystems/NAR_PIDSubsystem.java index af2c987..87029a0 100644 --- a/src/main/java/common/core/subsystems/NAR_PIDSubsystem.java +++ b/src/main/java/common/core/subsystems/NAR_PIDSubsystem.java @@ -4,8 +4,7 @@ import java.util.function.DoubleSupplier; import common.core.controllers.Controller; -import common.core.controllers.PController; -import common.core.controllers.VController; +import common.core.controllers.Controller.Type; import common.utility.NAR_Shuffleboard; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.PIDSubsystem; @@ -28,7 +27,7 @@ public abstract class NAR_PIDSubsystem extends SubsystemBase { /** * Creates a new PIDSubsystem. * - * @param controller the PIDController to use + * @param controller the Controller to use */ public NAR_PIDSubsystem(Controller controller) { m_controller = controller; @@ -62,20 +61,22 @@ public void initShuffleboard() { NAR_Shuffleboard.addData(getName(), "DEBUG", ()-> debug.getAsBoolean(), 2, 1); setpoint = NAR_Shuffleboard.debug(getName(), "Debug_Setpoint", 0, 2,2); - if (m_controller instanceof VController) { - m_controller.setkS(NAR_Shuffleboard.debug(getName(), "kS", m_controller.getkS(), 3, 0)); + if (m_controller.getType() == Type.DEFAULT) return; + + m_controller.setkS(NAR_Shuffleboard.debug(getName(), "kS", m_controller.getkS(), 3, 0)); + + if (m_controller.getType() == Type.VELOCITY) { m_controller.setkV(NAR_Shuffleboard.debug(getName(), "kV", m_controller.getkV(), 3, 1)); } - if (m_controller instanceof PController) { - m_controller.setkS(NAR_Shuffleboard.debug(getName(), "kS", m_controller.getkS(), 3, 0)); + else { m_controller.setkG(NAR_Shuffleboard.debug(getName(), "kG", m_controller.getkG(), 3, 1)); } } /** - * Returns the PIDController object controlling the subsystem + * Returns the Controller object controlling the subsystem * - * @return The PIDController + * @return The Controller */ public Controller getController() { return m_controller;