Elevator Example

Template Project

Start with the Command Robot template project. Elevators are implemented as subsystems, so the command-based structure is ideal. Create a new subsystem class in the subsystems package.

Overview

Elevators require Position Control. Unlike intakes (speed), elevators must move to precise heights and hold them against gravity. We'll build an elevator subsystem using PID control with Feedforward and Motion Profiling to achieve smooth, accurate movement.

Goals

The elevator subsystem must:

  • Move to setpoints (Low, Mid, High).
  • Hold position (Brake Mode + PID).
  • Use Motion Profiling (Smooth acceleration).
  • Compensate for gravity (Feedforward).

Step 1: Create Subsystem Structure

Start by creating the Elevator class that extends SubsystemBase. This class will manage the elevator motor and all configuration.
Create a new subsystem class in the subsystems package. The Command Robot template provides ExampleSubsystem as a reference. Create your Elevator class with necessary imports:
package frc.robot.subsystems;

import edu.wpi.first.wpilibj2.command.SubsystemBase;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.ctre.phoenix6.controls.MotionMagicVoltage;

public class Elevator extends SubsystemBase {
    // We'll add fields here
}

Step 2: Declare Motor and Control Objects

Add fields for the motor and control request. The motor is on CAN ID 6. For Talon FX, we create a MotionMagicVoltage control request that we'll reuse. For SPARK MAX, we get the closed-loop controller.
Add motor and controller fields:
public class Elevator extends SubsystemBase {
    // Elevator motor on CAN ID 6
    private final TalonFX m_motor = new TalonFX(6);
    
    // Motion Magic® control request (trapezoidal motion profile)
    private final MotionMagicVoltage m_request = new MotionMagicVoltage(0);
}

Step 3: Configure Motor - Basic Settings

In the constructor, configure the motor. Start with basic settings: Brake mode to hold position against gravity. This prevents the elevator from falling when power is removed.
Set idle mode to Brake:
public Elevator() {
    TalonFXConfiguration config = new TalonFXConfiguration();
    // Brake mode to hold position against gravity
    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
    m_motor.getConfigurator().apply(config);
}

Step 4: Configure PID Gains

Add PID gains for position control. kP (Proportional) corrects position errors. kD (Derivative) dampens oscillations. kI (Integral) is usually set to 0 for elevators. These values work with the motion profile to follow the trajectory accurately.
Add PID gains to the configuration:
public Elevator() {
    TalonFXConfiguration config = new TalonFXConfiguration();
    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
    
    // PID gains for position control (Slot 0)
    config.Slot0.kP = 0.5;  // Proportional gain
    config.Slot0.kI = 0.0;  // Integral gain
    config.Slot0.kD = 0.05; // Derivative gain
    
    m_motor.getConfigurator().apply(config);
}

Step 5: Add Feedforward Constants

Feedforward compensates for known forces. kG (Gravity) compensates for the elevator's weight pulling downward. kS (Static friction) overcomes initial friction to start moving. These values help the motor work less hard and move more smoothly.
Add feedforward constants:
public Elevator() {
    TalonFXConfiguration config = new TalonFXConfiguration();
    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
    config.Slot0.kP = 0.5;
    config.Slot0.kI = 0.0;
    config.Slot0.kD = 0.05;
    
    // Feedforward constants
    config.Slot0.kG = 0.12; // Gravity feedforward (compensates for weight)
    config.Slot0.kS = 0.05; // Static friction feedforward
    
    m_motor.getConfigurator().apply(config);
}

Step 6: Configure Motion Profile Constraints

Motion profiling smooths the movement. Set Max Velocity to the desired top speed and Max Acceleration to control how quickly it speeds up and slows down. Higher values mean faster movement but more stress on the mechanism.
Add motion profile constraints:
public Elevator() {
    TalonFXConfiguration config = new TalonFXConfiguration();
    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
    config.Slot0.kP = 0.5;
    config.Slot0.kI = 0.0;
    config.Slot0.kD = 0.05;
    config.Slot0.kG = 0.12;
    config.Slot0.kS = 0.05;
    
    // Motion profile constraints
    config.MotionMagic.MotionMagicCruiseVelocity = 10; // Max velocity (rotations/sec)
    config.MotionMagic.MotionMagicAcceleration = 20;   // Max acceleration (rotations/sec²)
    
    m_motor.getConfigurator().apply(config);
}

Step 7: Add Method to Set Target Height

Create a public method that commands the elevator to move to a specific position. The motion profile and PID controller handle the smooth movement automatically.
Add method to set target position:
/**
 * Set elevator to target height
 * @param rotations Target position in rotations
 */
public void setHeight(double rotations) {
    m_motor.setControl(m_request.withPosition(rotations));
}

Step 8: Create Commands in RobotContainer

In RobotContainer, bind buttons to commands that set the elevator to different heights. Use simple runOnce commands that call setHeight() with the desired position.
// In RobotContainer class - update the configureBindings() method

import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.subsystems.Elevator;

// The template provides RobotContainer with configureBindings()
private void configureBindings() {
    // ... other button bindings
    
    // Button A: Move to low position (0 rotations)
    m_controller.a().onTrue(new InstantCommand(() -> m_elevator.setHeight(0.0), m_elevator));
    // Button B: Move to high position (10 rotations)
    m_controller.b().onTrue(new InstantCommand(() -> m_elevator.setHeight(10.0), m_elevator));
}

Understanding the Complete System

When you call setHeight(), the motion profile generates a smooth trajectory from the current position to the target. The PID controller follows this trajectory, using feedforward to compensate for gravity and friction. The result is smooth, accurate movement that holds position at the target.

Full Code - Elevator Subsystem

Complete elevator subsystem implementation:
package frc.robot.subsystems;

import edu.wpi.first.wpilibj2.command.SubsystemBase;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.ctre.phoenix6.controls.MotionMagicVoltage;

public class Elevator extends SubsystemBase {
    // Elevator motor on CAN ID 6
    private final TalonFX m_motor = new TalonFX(6);
    
    // Motion Magic® control request (trapezoidal motion profile)
    private final MotionMagicVoltage m_request = new MotionMagicVoltage(0);

    public Elevator() {
        TalonFXConfiguration config = new TalonFXConfiguration();
        // Brake mode to hold position against gravity
        config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
        
        // PID gains for position control (Slot 0)
        config.Slot0.kP = 0.5;  // Proportional gain
        config.Slot0.kI = 0.0;  // Integral gain
        config.Slot0.kD = 0.05; // Derivative gain
        config.Slot0.kG = 0.12; // Gravity feedforward (compensates for weight)
        config.Slot0.kS = 0.05; // Static friction feedforward
        
        // Motion profile constraints
        config.MotionMagic.MotionMagicCruiseVelocity = 10; // Max velocity (rotations/sec)
        config.MotionMagic.MotionMagicAcceleration = 20;   // Max acceleration (rotations/sec²)
        
        m_motor.getConfigurator().apply(config);
    }
    
    /**
     * Set elevator to target height
     * @param rotations Target position in rotations
     */
    public void setHeight(double rotations) {
        m_motor.setControl(m_request.withPosition(rotations));
    }
}

Full Code - RobotContainer

// In RobotContainer class - update the configureBindings() method

import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.subsystems.Elevator;

// The template provides RobotContainer structure
private void configureBindings() {
    // ... other button bindings
    
    // Button A: Move to low position (0 rotations)
    m_controller.a().onTrue(new InstantCommand(() -> m_elevator.setHeight(0.0), m_elevator));
    // Button B: Move to high position (10 rotations)
    m_controller.b().onTrue(new InstantCommand(() -> m_elevator.setHeight(10.0), m_elevator));
}

Tuning Tips

Order of operations:

  • Gravity (kG): Increase until elevator holds position or falls very slowly (open loop).
  • Proportional (kP): Increase until it reaches target reasonably well.
  • Derivative (kD): Add to stop oscillation/overshoot.
  • Profile: Adjust Max Velocity/Acceleration for speed vs smoothness.

Practice with WPILib Documentation

WPILib provides a complete tuning tutorial for elevators with motion profiling. This interactive guide walks through the entire tuning process with examples and simulations. Use it to practice the concepts covered in this lesson.

Resources

PID ControlTrapezoidal Motion ProfilingWPILib: Tuning a Vertical Elevator (Practice)

Open full interactive app