feat: create elevator tunable for cruise velocity and max acceleration

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

Author: dabeycorn

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

@@ -26,7 +26,7 @@
 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.Constants.Flags;
 import org.frc6423.robot.Robot;
 
 /** Elevator Subsystem */
@@ -95,27 +95,35 @@ 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);
+  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 (!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();
+    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();
     }
   }
 
@@ -149,9 +157,16 @@ public void periodic() {
 
     filteredCurrent = currentFilter.calculate(hardware.getParentStatorCurrentAmps());
 
-    if (Constants.Flags.TUNE_MODE) {
+    if (Flags.TUNE_MODE) {
       hardware.setGains(
-          gainKg.get(), gainKs.get(), gainKv.get(), gainKa.get(), gainKp.get(), gainKd.get());
+          tunableKg.get(),
+          tunableKs.get(),
+          tunableKv.get(),
+          tunableKa.get(),
+          tunableKp.get(),
+          tunableKd.get(),
+          tunableMaxVel.get(),
+          tunableMaxAccel.get());
     }
   }
 
@@ -215,6 +230,7 @@ public Command runCurrentHoming() {
               if (!interupted) {
                 hardware.resetEncoders(0.0);
                 isZeroed = true;
+                System.out.println("Elevator Zeroed");
               }
             });
   }

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

@@ -77,8 +77,18 @@ public interface ElevatorIO extends AutoCloseable {
    * @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);
+  public void setGains(
+      double kG,
+      double kS,
+      double kV,
+      double kA,
+      double kP,
+      double kD,
+      double maxVel,
+      double maxAccel);
 
   /**
    * Set parent/child motors voltage setpoint

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

@@ -57,7 +57,15 @@ public double getChildTempCelsius() {
   public void resetEncoders(double poseMeters) {}
 
   @Override
-  public void setGains(double kG, double kS, double kV, double kA, double kP, double kD) {}
+  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/elevator/ElevatorIOReal.java

@@ -143,13 +143,23 @@ public void resetEncoders(double poseMeters) {
   }
 
   @Override
-  public void setGains(double kG, double kS, double kV, double kA, double kP, double kD) {
+  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(() -> parent.getConfigurator().apply(conf), 10, PARENT_MOTOR_ID);

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

@@ -35,9 +35,9 @@ public class ElevatorIOSim implements ElevatorIO {
 
   private double setpointPose;
 
-  private final ElevatorFeedforward feedforward = new ElevatorFeedforward(0.0, 0.1265, 0.4, 0.0);
+  private final ElevatorFeedforward feedforward = new ElevatorFeedforward(0.0, 0.127, 2.65, 0.0);
   private final ProfiledPIDController feedback =
-      new ProfiledPIDController(15.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2.25, 10.0));
+      new ProfiledPIDController(15.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2.25, 4.5));
 
   public ElevatorIOSim() {}
 
@@ -105,13 +105,22 @@ public double getChildTempCelsius() {
   public void resetEncoders(double poseMeters) {}
 
   @Override
-  public void setGains(double kG, double kS, double kV, double kA, double kP, double kD) {
+  public void setGains(
+      double kG,
+      double kS,
+      double kV,
+      double kA,
+      double kP,
+      double kD,
+      double maxVel,
+      double maxAccel) {
     feedforward.setKg(kG);
     feedforward.setKs(kS);
     feedforward.setKv(kV);
     feedforward.setKa(kA);
     feedback.setP(kP);
     feedback.setD(kD);
+    feedback.setConstraints(new TrapezoidProfile.Constraints(maxVel, maxAccel));
   }
 
   @Override