SystemController.getCoefficients

src/main/java/org/murraybridgebunyips/bunyipslib/external/ArmController.java

@@ -42,6 +42,11 @@ public ArmController(PIDController pid, ArmFeedforward ff, Supplier<Measure<Angl
         this.accelerationAngleProvider = accelerationAngleProvider;
     }
 
+    @Override
+    public double[] getCoefficients() {
+        return new double[]{pid.getP(), pid.getI(), pid.getD(), ff.getS(), ff.getCos(), ff.getV(), ff.getA()};
+    }
+
     @Override
     public void setCoefficients(double... coeffs) {
         if (coeffs.length != 7) {

src/main/java/org/murraybridgebunyips/bunyipslib/external/PIDFFController.java

@@ -32,6 +32,18 @@ public PIDFFController(PIDController pid, Encoder encoder, SystemController ff)
         this.ff = ff;
     }
 
+    @Override
+    public double[] getCoefficients() {
+        double[] ffCoeffs = ff.getCoefficients();
+        double[] pidCoeffs = pid.getCoefficients();
+
+        double[] coeffs = new double[ffCoeffs.length + pidCoeffs.length];
+        System.arraycopy(pidCoeffs, 0, coeffs, 0, pidCoeffs.length);
+        System.arraycopy(ffCoeffs, 0, coeffs, pidCoeffs.length, ffCoeffs.length);
+
+        return coeffs;
+    }
+
     @Override
     public void setCoefficients(double... coeffs) {
         if (coeffs.length < 3) {

src/main/java/org/murraybridgebunyips/bunyipslib/external/SystemController.java

@@ -11,8 +11,18 @@
  * @author Lucas Bubner, 2024
  */
 public interface SystemController {
+    /**
+     * Gets the current coefficients for this controller.
+     * The return of this method is expected to be in the same order and length as the set method.
+     *
+     * @return an array of coefficients for this controller
+     */
+    double[] getCoefficients();
+
     /**
      * Sets the coefficients for this controller.
+     * The order of the coefficients is expected to be the same as the return of the get method,
+     * specified internally by the controller.
      *
      * @param coeffs a list of coefficients to set this controller to
      */

src/main/java/org/murraybridgebunyips/bunyipslib/external/VelocityFFController.java

@@ -36,6 +36,11 @@ public VelocityFFController(PIDController pid, SimpleMotorFeedforward feedforwar
         this.bufferFraction = bufferFraction;
     }
 
+    @Override
+    public double[] getCoefficients() {
+        return new double[]{pid.getP(), pid.getI(), pid.getD(), ff.getS(), ff.getV(), ff.getA()};
+    }
+
     @Override
     public void setCoefficients(double... coeffs) {
         if (coeffs.length != 6) {

src/main/java/org/murraybridgebunyips/bunyipslib/external/ff/ArmFeedforward.java

@@ -185,6 +185,11 @@ public Measure<Velocity<Velocity<Angle>>> minAchievableAcceleration(Measure<Volt
         return maxAchievableAcceleration(maxVoltage.negate(), angle, velocity);
     }
 
+    @Override
+    public double[] getCoefficients() {
+        return new double[]{kS, kCos, kV, kA};
+    }
+
     @Override
     public void setCoefficients(double... coeffs) {
         if (coeffs.length != 4) {

src/main/java/org/murraybridgebunyips/bunyipslib/external/ff/ElevatorFeedforward.java

@@ -169,6 +169,11 @@ public double minAchievableAcceleration(double maxVoltage, double velocity) {
         return maxAchievableAcceleration(-maxVoltage, velocity);
     }
 
+    @Override
+    public double[] getCoefficients() {
+        return new double[]{kS, kG, kV, kA};
+    }
+
     @Override
     public void setCoefficients(double... coeffs) {
         if (coeffs.length != 4) {

src/main/java/org/murraybridgebunyips/bunyipslib/external/ff/SimpleMotorFeedforward.java

@@ -158,6 +158,11 @@ public double minAchievableAcceleration(double maxVoltage, double velocity) {
         return maxAchievableAcceleration(-maxVoltage, velocity);
     }
 
+    @Override
+    public double[] getCoefficients() {
+        return new double[]{kS, kV, kA};
+    }
+
     @Override
     public void setCoefficients(double... coeffs) {
         if (coeffs.length != 3) {

src/main/java/org/murraybridgebunyips/bunyipslib/external/pid/PIDFController.java

@@ -134,6 +134,7 @@ public boolean atSetPoint() {
     /**
      * @return the PIDF coefficients
      */
+    @Override
     public double[] getCoefficients() {
         return new double[]{kP, kI, kD, kF};
     }

src/main/java/org/murraybridgebunyips/bunyipslib/external/pid/ProfiledPIDController.java

@@ -299,6 +299,11 @@ public void reset(double measuredPosition) {
         reset(measuredPosition, 0.0);
     }
 
+    @Override
+    public double[] getCoefficients() {
+        return new double[]{getP(), getI(), getD()};
+    }
+
     @Override
     public void setCoefficients(double... coeffs) {
         controller.setCoefficients(coeffs);