Trapezoidal Profile Drive - Using SparkMax

My team and I are attempting to make code capable of driving the robot to a specifc TrapezoidalProfile State. We based our code off the WPILib example here.
Here is our full DriveSubsytem, and the relevant code is below:

  private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(

  public void setDriveStates(TrapezoidProfile.State left, TrapezoidProfile.State right) {
leftMotor1.getPIDController().setReference(left.position, ControlType.kPosition, 0,

    leftMotor1.getPIDController().setReference(right.position, ControlType.kPosition, 0,

The profile is executed through a class that extends TrapezoidalProfileCommand, below:

   package frc.robot.commands;

    import edu.wpi.first.math.trajectory.TrapezoidProfile;
    import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
    import frc.robot.Constants.AutoConstants;
    import frc.robot.RobotContainer.Subsystems;

    /** Drives a set distance using a motion profile. */
    public class TrapezoidDriveCommand extends TrapezoidProfileCommand {
       * Creates a new DriveDistanceProfiled command.
       * @param meters The distance to drive.
       * @param drive The drive subsystem to use.
      public TrapezoidDriveCommand(double meters) {
        new TrapezoidProfile(
            // Limit the max acceleration and velocity
            new TrapezoidProfile.Constraints(
            // End at desired position in meters; implicitly starts at 0
            new TrapezoidProfile.State(meters, 0)),
        // Pipe the profile state to the drive
        setpointState -> Subsystems.driveSubsystem.setDriveStates(setpointState, setpointState),
        // Require the drive
    // Reset drive encoders since we're starting at 0


When we run the code to drive to a distance of 2 meters, it drives an extremely short distance and the motors stutter. We can see by the SparkMax LED’s that they are rapidly getting forward and backwards commands. I’m unsure if there is an issue in our implementation, or in our constants, but if anyone with expertise on the WPILib Trapezoidal classes and SparkMax’s could help, we would appreciate it.

Put as much of the relevant telemetry on your dashboard as you can. You need to see what the setpoints and measurements are at each point in your control stack.

1 Like

We are able to print the setpoints, but I’m unsure what values to compare them to. In fact, looking over the WPILib example, I’m a little confused on how it works. The TrapezoidalCommand constructor doesn’t take in the distances from our encoders, so how is it getting feedback that it has reached the specified distance?

It doesn’t. That’s the job of your PID controllers. All the trapezoidal profile generator does is limit the velocity and acceleration of the reference position.


Ok, so in that case the Spark Max’s should get feedback from their RelativeEncoder. We set the conversions for that RelativeEncoder and ran the above code, but the same problem persists.

What are your PID constants? It sounds like the controller is oscillating violently due to excessive gains. Start by reducing kP very low then raising it until you start getting a response

Make sure you include a feedforward; pure feedback control is not a very good control strategy.

1 Like

Just going to make sure that this isn’t the issue; you’re running Position PID and adding a velocity feedforward. If I understand correctly how profiles work, you should be running velocity PID+FF tracking the profile-supplied setpoint. I might be wrong though; I’m happy to be corrected on this.

If this is velocity PID, would the 112ms internal NEO encoder velocity filtering delay be an issue for a drivetrain or an elevator? I know it is a major problem for flywheels.

1 Like

It may be unrelated but seeing your description reminded me of this: Spark MAX controllers stuttering - #21 by adias

You don’t want to call getPIDController() repeatedly. Call it once for each motor controller and stash the result and re-use it. It’s at least worth eliminating as a possible problem.

Did you do the Robot Characteristization (or SysID) which helps you arrive at your PID constants? Each robot will be different from motor, motor controller, encoders, gear ratio, width, length, etc etc so you can’t just use other ppl’s drive train constants and assume they’ll just work.

Keep in mind the SysID tool generates velocity feedback gains by default, so if you go this route you’ll want to make sure Position is selected for loop type.

Out of curiosity, how does the feed forward for a position PID work?

I understand on a velocity, the feed forward scales with the desired input. I.e., set a feed forward that gets you near the output when you set the Input.

But how does the math work for a feed forward for a positional PID? Anything that scales with the distance from your goal seems like it would be a duplication of the P term.


If you have a desired velocity as well as position (as is the case for motion profiles), you can calculate feedforward based on that. Feedforward for a purely positional reference is usually only used for compensation against gravity.


Ok - that makes perfect sense. So if, for instance, I’m using straight PID on a swerve module steering azimuth but not using motion profiling, feed forward doesn’t exist.

1 Like

You could hack in a kS feedforward based on the direction of motion in that situation, but if your azimuthal motor is sufficiently overpowered it won’t really matter.

1 Like

We fixed an error in our encoder conversions, and now we have a different problem. When constructed for a 1 meter distance, the trapezoidal profile (left graph) gives setpoints correctly. However, the Spark Max (right graph is encoder distance), goes to approximately double the distance. Any ideas?

Ok looks like it wasn’t reaching setpoints because the P was too low.

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.