feat: add tunable entries for the elevator subsystem

Signed-off-by: Dasun Abeykoon <[email protected]>

Author: dabeycorn

src/main/java/org/frc6423/robot/subsystems/superstructure/elevator/Elevator.java

@@ -17,13 +17,16 @@
 import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.networktables.DoubleEntry;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.units.measure.LinearAcceleration;
 import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.units.measure.Mass;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import java.util.function.DoubleSupplier;
+import org.frc6423.lib.utilities.NtUtils;
+import org.frc6423.robot.Constants;
 import org.frc6423.robot.Robot;
 
 /** Elevator Subsystem */
@@ -91,6 +94,31 @@ public static enum ElevatorExtension {
 
   private boolean isZeroed = false;
 
+  private static final String gainTopic = "Tunables/elevator";
+  private static final DoubleEntry gainKg = NtUtils.createDoubleEntry(gainTopic + "/kG", 0.0);
+  private static final DoubleEntry gainKs = NtUtils.createDoubleEntry(gainTopic + "/kS", 0.0);
+  private static final DoubleEntry gainKv = NtUtils.createDoubleEntry(gainTopic + "/kV", 0.0);
+  private static final DoubleEntry gainKa = NtUtils.createDoubleEntry(gainTopic + "/kA", 0.0);
+  private static final DoubleEntry gainKp = NtUtils.createDoubleEntry(gainTopic + "/kP", 0.0);
+  private static final DoubleEntry gainKd = NtUtils.createDoubleEntry(gainTopic + "/kD", 0.0);
+
+  static {
+    if (!Constants.Flags.TUNE_MODE) {
+      gainKg.unpublish();
+      gainKs.unpublish();
+      gainKv.unpublish();
+      gainKa.unpublish();
+      gainKp.unpublish();
+      gainKd.unpublish();
+      gainKg.close();
+      gainKs.close();
+      gainKv.close();
+      gainKa.close();
+      gainKp.close();
+      gainKd.close();
+    }
+  }
+
   /**
    * @return fake {@link Elevator} subsystem
    */
@@ -120,6 +148,11 @@ public void periodic() {
     hardware.periodic();
 
     filteredCurrent = currentFilter.calculate(hardware.getParentStatorCurrentAmps());
+
+    if (Constants.Flags.TUNE_MODE) {
+      hardware.setGains(
+          gainKg.get(), gainKs.get(), gainKv.get(), gainKa.get(), gainKp.get(), gainKd.get());
+    }
   }
 
   /**

src/main/java/org/frc6423/robot/subsystems/superstructure/elevator/ElevatorIO.java

@@ -62,6 +62,18 @@ public interface ElevatorIO extends AutoCloseable {
    */
   public void resetEncoders(double poseMeters);
 
+  /**
+   * Set elevator gains for onboard feedforward and feedback control
+   *
+   * @param kG output to overcome gravity
+   * @param kS output to overcome static friction
+   * @param kV output per unit of target velocity (output/mps)
+   * @param kA output per unit of target acceleration (output/(mps/s))
+   * @param kP output per unit of error in position (meters)
+   * @param kD output per unit of error in velocity (mps)
+   */
+  public void setGains(double kG, double kS, double kV, double kA, double kP, double kD);
+
   /**
    * Set parent/child motors voltage setpoint
    *

src/main/java/org/frc6423/robot/subsystems/superstructure/elevator/ElevatorIONone.java

@@ -51,6 +51,9 @@ public double getChildTempCelsius() {
   @Override
   public void resetEncoders(double poseMeters) {}
 
+  @Override
+  public void setGains(double kG, double kS, double kV, double kA, double kP, double kD) {}
+
   @Override
   public void setVolts(double volts) {}
 

src/main/java/org/frc6423/robot/subsystems/superstructure/elevator/ElevatorIOReal.java

@@ -134,6 +134,19 @@ public void resetEncoders(double poseMeters) {
     parent.setPosition(poseMeters);
   }
 
+  @Override
+  public void setGains(double kG, double kS, double kV, double kA, double kP, double kD) {
+    conf.Slot0.kG = kG;
+    conf.Slot0.kS = kS;
+    conf.Slot0.kV = kV;
+    conf.Slot0.kA = kA;
+    conf.Slot0.kP = kP;
+    conf.Slot0.kD = kD;
+
+    parent.getConfigurator().apply(conf);
+    child.getConfigurator().apply(conf);
+  }
+
   @Override
   public void setVolts(double volts) {
     parent.setControl(voltReq.withOutput(volts).withEnableFOC(true));

src/main/java/org/frc6423/robot/subsystems/superstructure/elevator/ElevatorIOSim.java

@@ -99,6 +99,16 @@ public double getChildTempCelsius() {
   @Override
   public void resetEncoders(double poseMeters) {}
 
+  @Override
+  public void setGains(double kG, double kS, double kV, double kA, double kP, double kD) {
+    feedforward.setKg(kG);
+    feedforward.setKs(kS);
+    feedforward.setKv(kV);
+    feedforward.setKa(kA);
+    feedback.setP(kP);
+    feedback.setD(kD);
+  }
+
   @Override
   public void setVolts(double volts) {
     appliedVolts = MathUtil.clamp(volts, -12.0, 12.0);