Motion Profiling: Trapezoidal
Template Project
Start with the Command Robot template project. Motion profiling is typically implemented in subsystems, so the command-based structure works well. Create a new subsystem class in the
subsystems package.Overview
Trapezoidal motion profiling creates smooth movement using constant acceleration to reach maximum velocity, then constant deceleration to stop. This profile has three phases: acceleration, cruise (constant speed), and deceleration. It's fast, predictable, and reduces mechanical stress compared to instant speed changes.
What is Trapezoidal Profiling?
A trapezoidal profile accelerates at a constant rate until reaching maximum velocity, maintains that speed, then decelerates at a constant rate to stop. The name comes from the velocity graph: it looks like a trapezoid when plotted over time.
Hardware Support
Built-in controller features:
- REV (SPARK MAX): Uses
MAXMotion(Trapezoidal). - CTRE (Talon FX): Uses
Motion Magic®(Trapezoidal/S-Curve).
Step 1: Understand Configuration Parameters
Two main parameters control the profile: Max Velocity sets the top speed during the cruise phase. Max Acceleration determines how quickly the mechanism speeds up and slows down. Optional Jerk (for Talon FX) smooths the acceleration transitions.
Step 2: Configure Motion Profile Constraints
We configure the motion profile constraints in the motor configuration. These values are typically set in rotations per second (RPS) for velocity and rotations per second squared (RPS²) for acceleration. Start with conservative values and tune based on your mechanism's capabilities.
Configure motion profile constraints:
public void configureMotionProfile() {
SparkMaxConfig config = new SparkMaxConfig();
// Configure motion profile constraints for Slot 0
// Max velocity in RPM (adjust based on your mechanism)
config.closedLoop.maxMotion
.maxVelocity(3000, ClosedLoopSlot.kSlot0)
// Max acceleration in RPM/s
.maxAcceleration(2000, ClosedLoopSlot.kSlot0);
motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
}Step 3: Use Motion Profile Control
Once configured, use the motion profile control mode when setting positions. The controller automatically generates the trapezoidal profile based on your constraints and the current position.
Set position using motion profile:
public void setPosition(double target) {
// Use MAXMotion position control (trapezoidal profile)
controller.setReference(target, ControlType.kMAXMotionPositionControl);
}Tuning Trapezoidal Profiles
Tuning a trapezoidal profile requires balancing speed and smoothness. Start by determining your mechanism's maximum safe speed and acceleration, then adjust based on performance.
Tuning Process
Follow this order:
- 1. Find Maximum Velocity: Set acceleration very high, gradually increase velocity until the mechanism struggles to maintain speed or hits physical limits. Back off 10-20% for safety.
- 2. Find Maximum Acceleration: Keep velocity at your maximum, increase acceleration until you see overshoot, vibration, or mechanical stress. Reduce by 15-25%.
- 3. Balance for Performance: If movement is too slow, increase both values proportionally. If there's overshoot or vibration, reduce acceleration first.
Tuning Tips
Use dashboard tools to monitor position, velocity, and current draw during motion. The velocity graph should form a clean trapezoid: sharp ramp up, flat top, sharp ramp down. Oscillation or overshoot indicates acceleration is too high. Slow, sluggish movement means values are too conservative.
Common Issues and Solutions
Overshoot at end: Reduce acceleration, increase PID derivative gain, or add jerk smoothing (Talon FX). Too slow: Increase velocity and acceleration together, but verify mechanism can handle it. Vibration during motion: Reduce acceleration significantly, check for mechanical issues. Can't reach target: Increase max velocity, check if distance is achievable with current constraints.
Full Code Example
Complete example combining configuration and usage:
package frc.robot.subsystems;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.config.SparkMaxConfig;
import com.revrobotics.spark.ClosedLoopController.ControlType;
import com.revrobotics.spark.ClosedLoopController.ClosedLoopSlot;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkBase.PersistMode;
// Create this class in the subsystems package (template provides ExampleSubsystem as reference)
public class Elevator extends SubsystemBase {
private SparkMax motor = new SparkMax(1, MotorType.kBrushless);
private SparkClosedLoopController controller = motor.getClosedLoopController();
public Elevator() {
configureMotionProfile();
}
public void configureMotionProfile() {
SparkMaxConfig config = new SparkMaxConfig();
// Configure motion profile constraints for Slot 0
config.closedLoop.maxMotion
.maxVelocity(3000, ClosedLoopSlot.kSlot0) // Max velocity in RPM
.maxAcceleration(2000, ClosedLoopSlot.kSlot0); // Max acceleration in RPM/s
motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
}
public void setPosition(double target) {
// Use MAXMotion position control (trapezoidal profile)
controller.setReference(target, ControlType.kMAXMotionPositionControl);
}
}