From be3f21b2e09f7b93201d2d594b57311d2d20ea4d Mon Sep 17 00:00:00 2001
From: Lucas Bubner <lkbubner@proton.me>
Date: Tue, 13 Aug 2024 23:45:27 +0930
Subject: [PATCH] SystemController.getCoefficients

---
 .../bunyipslib/external/ArmController.java           |  5 +++++
 .../bunyipslib/external/PIDFFController.java         | 12 ++++++++++++
 .../bunyipslib/external/SystemController.java        | 10 ++++++++++
 .../bunyipslib/external/VelocityFFController.java    |  5 +++++
 .../bunyipslib/external/ff/ArmFeedforward.java       |  5 +++++
 .../bunyipslib/external/ff/ElevatorFeedforward.java  |  5 +++++
 .../external/ff/SimpleMotorFeedforward.java          |  5 +++++
 .../bunyipslib/external/pid/PIDFController.java      |  1 +
 .../external/pid/ProfiledPIDController.java          |  5 +++++
 9 files changed, 53 insertions(+)

diff --git a/src/main/java/org/murraybridgebunyips/bunyipslib/external/ArmController.java b/src/main/java/org/murraybridgebunyips/bunyipslib/external/ArmController.java
index 50cb94223..ff1113f88 100644
--- a/src/main/java/org/murraybridgebunyips/bunyipslib/external/ArmController.java
+++ b/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) {
diff --git a/src/main/java/org/murraybridgebunyips/bunyipslib/external/PIDFFController.java b/src/main/java/org/murraybridgebunyips/bunyipslib/external/PIDFFController.java
index 00f8c00c3..fc357e49a 100644
--- a/src/main/java/org/murraybridgebunyips/bunyipslib/external/PIDFFController.java
+++ b/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) {
diff --git a/src/main/java/org/murraybridgebunyips/bunyipslib/external/SystemController.java b/src/main/java/org/murraybridgebunyips/bunyipslib/external/SystemController.java
index f25fb0d49..d50608eb5 100644
--- a/src/main/java/org/murraybridgebunyips/bunyipslib/external/SystemController.java
+++ b/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
      */
diff --git a/src/main/java/org/murraybridgebunyips/bunyipslib/external/VelocityFFController.java b/src/main/java/org/murraybridgebunyips/bunyipslib/external/VelocityFFController.java
index 4f8f214d5..dc7ac8b5a 100644
--- a/src/main/java/org/murraybridgebunyips/bunyipslib/external/VelocityFFController.java
+++ b/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) {
diff --git a/src/main/java/org/murraybridgebunyips/bunyipslib/external/ff/ArmFeedforward.java b/src/main/java/org/murraybridgebunyips/bunyipslib/external/ff/ArmFeedforward.java
index f240dfcd4..e0826d944 100644
--- a/src/main/java/org/murraybridgebunyips/bunyipslib/external/ff/ArmFeedforward.java
+++ b/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) {
diff --git a/src/main/java/org/murraybridgebunyips/bunyipslib/external/ff/ElevatorFeedforward.java b/src/main/java/org/murraybridgebunyips/bunyipslib/external/ff/ElevatorFeedforward.java
index 9842267aa..348b75a00 100644
--- a/src/main/java/org/murraybridgebunyips/bunyipslib/external/ff/ElevatorFeedforward.java
+++ b/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) {
diff --git a/src/main/java/org/murraybridgebunyips/bunyipslib/external/ff/SimpleMotorFeedforward.java b/src/main/java/org/murraybridgebunyips/bunyipslib/external/ff/SimpleMotorFeedforward.java
index a6ac47fc5..5ecc68276 100644
--- a/src/main/java/org/murraybridgebunyips/bunyipslib/external/ff/SimpleMotorFeedforward.java
+++ b/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) {
diff --git a/src/main/java/org/murraybridgebunyips/bunyipslib/external/pid/PIDFController.java b/src/main/java/org/murraybridgebunyips/bunyipslib/external/pid/PIDFController.java
index 0bf092228..f6346f4ee 100644
--- a/src/main/java/org/murraybridgebunyips/bunyipslib/external/pid/PIDFController.java
+++ b/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};
     }
diff --git a/src/main/java/org/murraybridgebunyips/bunyipslib/external/pid/ProfiledPIDController.java b/src/main/java/org/murraybridgebunyips/bunyipslib/external/pid/ProfiledPIDController.java
index fe610b14f..076fc64c9 100644
--- a/src/main/java/org/murraybridgebunyips/bunyipslib/external/pid/ProfiledPIDController.java
+++ b/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);