Make Controllers Sendable

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);
     }
 

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.
@@ -15,16 +20,35 @@
  */
 public class ControllerGroup extends Controller {
 
-    /** Controller part of the group */
-    private final Controller mController;
-
     /** Controllers part of the group */
-    private final Controller[] mControllers;
+    private final ArrayList<Controller> mControllers;
 
     /** Create a controller group */
     public ControllerGroup(Controller controller, Controller... controllers) {
-        mController = controller;
-        mControllers = controllers;
+        mControllers = new ArrayList<>();
+
+        mControllers.add(controller);
+        for (Controller tmpController : controllers) {
+            mControllers.add(tmpController);
+        }
+    }
+
+    @Override
+    public void initSendable(SendableBuilder 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;
     }
 
     /**
@@ -37,7 +61,7 @@ public ControllerGroup(Controller controller, Controller... controllers) {
      */
     @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);

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("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);
+    }
+
     /**
      * Set the setpoint filter of the controller
      *

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.
@@ -17,16 +21,28 @@
  */
 public class AngleControllerGroup extends AngleController {
 
-    /** Controller part of the group */
-    private final AngleController mController;
-
     /** Controllers part of the group */
-    private final AngleController[] mControllers;
+    private final ArrayList<AngleController> mControllers;
 
     /** Create a controller group */
     public AngleControllerGroup(AngleController controller, AngleController... controllers) {
-        mController = controller;
-        mControllers = controllers;
+
+        mControllers = new ArrayList<>(controllers.length);
+        mControllers.add(controller);
+
+        for (AngleController tmpController : controllers) {
+            if (tmpController == null) {
+                throw new IllegalArgumentException("Controller cannot be null");
+            }
+            mControllers.add(tmpController);
+        }
+    }
+
+    @Override
+    public void initSendable(SendableBuilder builder) {
+        for (AngleController controller : mControllers) {
+            controller.initSendable(builder);
+        }
     }
 
     /**
@@ -39,7 +55,7 @@ public AngleControllerGroup(AngleController controller, AngleController... contr
      */
     @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);

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("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);
+    }
+
     /**
      * @param speed sets speed for motor output of controller
      * @return the calculated result from the PIDController

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("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 */
     public AnglePIDController() {
         this(-1, -1, -1);

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("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);
+    }
+
     /**
      * Calculates voltage to hold arm at the setpoint angle
      *

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("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);
+    }
+
     /**
      * Calculates a motor output by feeding the derivative of a positional setpoint to a feedforward
      * model

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("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);
+    }
+
     /**
      * @param speed sets speed for motor output of controller
      * @return the calculated result from the PIDController

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("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 */
     public PIDController() {
         this(-1, -1, -1);

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("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);
+    }
+
     /**
      * Calculates voltage to hold arm at the setpoint angle
      *

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("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);
+    }
+
     /**
      * Calculate voltage to hold elevator at setpoint height
      *