Mini-Project: A Closed-Loop Elevator with Motion Magic

Let's build a two-stage elevator driven by a Kraken X60 running CTRE's integrated Talon FX controller. The goal: command the elevator to named heights (stow, L2, L4) and have it slew there on a trapezoidal profile using Motion Magic, Phoenix 6's onboard motion-profiled position control.

Configure the motor

Motion Magic needs a Slot 0 PID set plus a feedforward (kS/kV/kA) and the profile limits (cruise velocity, acceleration, jerk). All of it lives in a TalonFXConfiguration:

private final TalonFX m_motor = new TalonFX(20); // CAN ID 20
private final MotionMagicVoltage m_request = new MotionMagicVoltage(0);

public Elevator() {
  var cfg = new TalonFXConfiguration();
  var slot0 = cfg.Slot0;
  slot0.kS = 0.25;  // volts to overcome static friction
  slot0.kV = 0.12;  // volts per rotation/sec
  slot0.kA = 0.01;  // volts per rotation/sec^2
  slot0.kG = 0.30;  // gravity feedforward (elevator type)
  slot0.kP = 4.8;
  slot0.kI = 0;
  slot0.kD = 0.1;
  slot0.GravityType = GravityTypeValue.Elevator_Static;

  var mm = cfg.MotionMagic;
  mm.MotionMagicCruiseVelocity = 80;   // rotations per second
  mm.MotionMagicAcceleration   = 160;  // rps per second
  mm.MotionMagicJerk           = 1600; // rps per second^2

  m_motor.getConfigurator().apply(cfg);
}

GravityTypeValue.Elevator_Static is the correct value for a vertical lift (use Arm_Cosine instead for a rotating arm, where the gravity term scales with the cosine of the angle).

Convert rotations to inches

Your mechanism doesn't think in motor rotations -- it thinks in inches of travel. If a 24-tooth pulley with 5 mm pitch moves the carriage, and the gearbox is 9:1, one motor rotation = (24 x 5 mm) / 9 = 13.33 mm = 0.525 in. Store that constant and convert at the boundary:

private static final double kRotationsPerInch = 9.0 / (24 * 5.0 / 25.4);

public Command goToHeight(double inches) {
  double targetRot = inches * kRotationsPerInch;
  return run(() -> m_motor.setControl(m_request.withPosition(targetRot)))
      .until(() -> Math.abs(
          m_motor.getPosition().getValueAsDouble() - targetRot) < 0.5);
}

Wire it to a button

In RobotContainer, expose named heights as command factories and bind them:

m_driver.a().onTrue(m_elevator.goToHeight(0.0));   // stow
m_driver.b().onTrue(m_elevator.goToHeight(31.5));  // L2
m_driver.y().onTrue(m_elevator.goToHeight(72.0));  // L4

Tune in this order

1. Set kP=kI=kD=0 and only kG until the carriage holds position against gravity.
2. Add kV (from a SysId run or 12 / free-speed-rps), then kS.
3. Bring up kP until it tracks the profile crisply; add a little kD only if it overshoots.

Because Motion Magic generates the trajectory on the motor controller at 1 kHz, your roboRIO loop just sends a target rotation -- no 20 ms profile math on the RIO. Watch the position-vs-setpoint plot in AdvantageScope while tuning; the measured curve should hug the generated profile with no lag.