feat: create arm tunables for motion magic control

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

Author: dabeycorn

src/main/java/org/frc6423/robot/subsystems/superstructure/arm/Arm.java

@@ -17,6 +17,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.networktables.DoubleEntry;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularAcceleration;
 import edu.wpi.first.units.measure.AngularVelocity;
@@ -25,6 +26,8 @@
 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.Flags;
 import org.frc6423.robot.Robot;
 
 /** Arm Subsystem */
@@ -91,6 +94,39 @@ private ArmAngle(Angle angle) {
 
   private boolean isZeroed = false;
 
+  private static final String gainTopic = "Tunables/elevator";
+  private static final DoubleEntry tunableKg = NtUtils.createDoubleEntry(gainTopic + "/kG", 0.0);
+  private static final DoubleEntry tunableKs = NtUtils.createDoubleEntry(gainTopic + "/kS", 0.0);
+  private static final DoubleEntry tunableKv = NtUtils.createDoubleEntry(gainTopic + "/kV", 0.0);
+  private static final DoubleEntry tunableKa = NtUtils.createDoubleEntry(gainTopic + "/kA", 0.0);
+  private static final DoubleEntry tunableKp = NtUtils.createDoubleEntry(gainTopic + "/kP", 0.0);
+  private static final DoubleEntry tunableKd = NtUtils.createDoubleEntry(gainTopic + "/kD", 0.0);
+  private static final DoubleEntry tunableMaxVel =
+      NtUtils.createDoubleEntry(gainTopic + "/maxVel", 0.0);
+  private static final DoubleEntry tunableMaxAccel =
+      NtUtils.createDoubleEntry(gainTopic + "/maxAccel", 0.0);
+
+  static {
+    if (!Flags.TUNE_MODE) {
+      tunableKg.unpublish();
+      tunableKs.unpublish();
+      tunableKv.unpublish();
+      tunableKa.unpublish();
+      tunableKp.unpublish();
+      tunableKd.unpublish();
+      tunableMaxVel.unpublish();
+      tunableMaxAccel.unpublish();
+      tunableKg.close();
+      tunableKs.close();
+      tunableKv.close();
+      tunableKa.close();
+      tunableKp.close();
+      tunableKd.close();
+      tunableMaxVel.close();
+      tunableMaxAccel.close();
+    }
+  }
+
   /**
    * @return fake {@link Arm} subsystem
    */
@@ -120,6 +156,18 @@ public void periodic() {
     hardware.periodic();
 
     filteredCurrent = currentFilter.calculate(hardware.getStatorCurrentAmps());
+
+    if (Flags.TUNE_MODE) {
+      hardware.setGains(
+          tunableKg.get(),
+          tunableKs.get(),
+          tunableKv.get(),
+          tunableKa.get(),
+          tunableKp.get(),
+          tunableKd.get(),
+          tunableMaxVel.get(),
+          tunableMaxAccel.get());
+    }
   }
 
   /**
@@ -161,6 +209,7 @@ public Command runCurrentHoming() {
               if (!interupted) {
                 hardware.resetEncoder(0.0);
                 isZeroed = true;
+                System.out.println("Arm Homed!");
               }
             });
   }

src/main/java/org/frc6423/robot/subsystems/superstructure/arm/ArmIO.java

@@ -38,6 +38,28 @@ public interface ArmIO extends AutoCloseable {
    */
   public void resetEncoder(double poseRads);
 
+  /**
+   * Set arm 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)
+   * @param maxVel max velocity of motion profile
+   * @param maxAccel max acceleration of motion profile
+   */
+  public void setGains(
+      double kG,
+      double kS,
+      double kV,
+      double kA,
+      double kP,
+      double kD,
+      double maxVel,
+      double maxAccel);
+
   /**
    * Set pivot motor voltage setpoint
    *

src/main/java/org/frc6423/robot/subsystems/superstructure/arm/ArmIONone.java

@@ -31,6 +31,17 @@ public double getStatorCurrentAmps() {
   @Override
   public void resetEncoder(double poseRads) {}
 
+  @Override
+  public void setGains(
+      double kG,
+      double kS,
+      double kV,
+      double kA,
+      double kP,
+      double kD,
+      double maxVel,
+      double maxAccel) {}
+
   @Override
   public void setVolts(double volts) {}
 

src/main/java/org/frc6423/robot/subsystems/superstructure/arm/ArmIOReal.java

@@ -9,6 +9,7 @@
 import static edu.wpi.first.units.Units.Radians;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
+import static org.frc6423.lib.utilities.CtreUtils.tryUntilOk;
 import static org.frc6423.robot.subsystems.superstructure.arm.Arm.CANBUS;
 import static org.frc6423.robot.subsystems.superstructure.arm.Arm.GEAR_REDUCTION;
 import static org.frc6423.robot.subsystems.superstructure.arm.Arm.MAX_ACCELERATION;
@@ -68,7 +69,7 @@ public ArmIOReal() {
     conf.MotionMagic.MotionMagicCruiseVelocity = MAX_VELOCITY.in(RadiansPerSecond);
     conf.MotionMagic.MotionMagicAcceleration = MAX_ACCELERATION.in(RadiansPerSecondPerSecond);
 
-    motor.getConfigurator().apply(conf);
+    tryUntilOk(() -> motor.getConfigurator().apply(conf), 10, MOTOR_ID);
 
     poseSig = motor.getPosition();
     currentSig = motor.getStatorCurrent();
@@ -99,6 +100,30 @@ public void resetEncoder(double poseRads) {
     motor.setPosition(poseRads);
   }
 
+  @Override
+  public void setGains(
+      double kG,
+      double kS,
+      double kV,
+      double kA,
+      double kP,
+      double kD,
+      double maxVel,
+      double maxAccel) {
+
+    conf.Slot0.kG = kG;
+    conf.Slot0.kS = kS;
+    conf.Slot0.kV = kV;
+    conf.Slot0.kA = kA;
+    conf.Slot0.kP = kP;
+    conf.Slot0.kD = kD;
+    conf.MotionMagic.MotionMagicCruiseVelocity = maxVel;
+    conf.MotionMagic.MotionMagicAcceleration = maxAccel;
+
+    // I think the configurator will only apply if the config is different so this should be fine
+    tryUntilOk(() -> motor.getConfigurator().apply(conf), 10, MOTOR_ID);
+  }
+
   @Override
   public void setVolts(double volts) {
     motor.setControl(voltReq.withOutput(volts).withEnableFOC(true));

src/main/java/org/frc6423/robot/subsystems/superstructure/arm/ArmIOSim.java

@@ -72,6 +72,17 @@ public double getStatorCurrentAmps() {
   @Override
   public void resetEncoder(double poseRads) {}
 
+  @Override
+  public void setGains(
+      double kG,
+      double kS,
+      double kV,
+      double kA,
+      double kP,
+      double kD,
+      double maxVel,
+      double maxAccel) {}
+
   @Override
   public void setVolts(double volts) {
     pivotAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0);

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

@@ -230,7 +230,7 @@ public Command runCurrentHoming() {
               if (!interupted) {
                 hardware.resetEncoders(0.0);
                 isZeroed = true;
-                System.out.println("Elevator Zeroed");
+                System.out.println("Elevator Homed!");
               }
             });
   }