PID Control in Command-based

Note

For a description of the WPILib PID control features used by these command-based wrappers, see PID Control in WPILib.

One of the most common control algorithms used in FRC® and FTC® is the PID controller. WPILib offers its own PIDController class to help teams implement this functionality on their robots. The following example is from the RapidReactCommandBot example project (Java, C++) and shows how PIDControllers can be used within the command-based framework:

Java

package org.wpilib.examples.rapidreactcommandbot.subsystems;

import static org.wpilib.command2.Commands.parallel;
import static org.wpilib.command2.Commands.waitUntil;

import org.wpilib.command2.Command;
import org.wpilib.command2.SubsystemBase;
import org.wpilib.epilogue.Logged;
import org.wpilib.examples.rapidreactcommandbot.Constants.ShooterConstants;
import org.wpilib.hardware.motor.PWMSparkMax;
import org.wpilib.hardware.rotation.Encoder;
import org.wpilib.math.controller.PIDController;
import org.wpilib.math.controller.SimpleMotorFeedforward;

@Logged
public class Shooter extends SubsystemBase {
  private final PWMSparkMax shooterMotor = new PWMSparkMax(ShooterConstants.kShooterMotorPort);
  private final PWMSparkMax feederMotor = new PWMSparkMax(ShooterConstants.kFeederMotorPort);
  private final Encoder shooterEncoder =
      new Encoder(
          ShooterConstants.kEncoderPorts[0],
          ShooterConstants.kEncoderPorts[1],
          ShooterConstants.kEncoderReversed);
  private final SimpleMotorFeedforward shooterFeedforward =
      new SimpleMotorFeedforward(ShooterConstants.kS, ShooterConstants.kV);
  private final PIDController shooterFeedback = new PIDController(ShooterConstants.kP, 0.0, 0.0);

  /** The shooter subsystem for the robot. */
  public Shooter() {
    shooterFeedback.setTolerance(ShooterConstants.kShooterToleranceRPS);
    shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse);

    // Set default command to turn off both the shooter and feeder motors, and then idle
    setDefaultCommand(
        runOnce(
                () -> {
                  shooterMotor.disable();
                  feederMotor.disable();
                })
            .andThen(run(() -> {}))
            .withName("Idle"));
  }

  /**
   * Returns a command to shoot the balls currently stored in the robot. Spins the shooter flywheel
   * up to the specified setpoint, and then runs the feeder motor.
   *
   * @param setpointRotationsPerSecond The desired shooter velocity
   */
  public Command shootCommand(double setpointRotationsPerSecond) {
    return parallel(
            // Run the shooter flywheel at the desired setpoint using feedforward and feedback
            run(
                () -> {
                  shooterMotor.setThrottle(
                      shooterFeedforward.calculate(setpointRotationsPerSecond)
                          + shooterFeedback.calculate(
                              shooterEncoder.getRate(), setpointRotationsPerSecond));
                }),

            // Wait until the shooter has reached the setpoint, and then run the feeder
            waitUntil(shooterFeedback::atSetpoint).andThen(() -> feederMotor.setThrottle(1)))
        .withName("Shoot");
  }
}

C++

#pragma once

#include <functional>

#include "Constants.hpp"
#include "wpi/commands2/CommandPtr.hpp"
#include "wpi/commands2/SubsystemBase.hpp"
#include "wpi/hardware/motor/PWMSparkMax.hpp"
#include "wpi/hardware/rotation/Encoder.hpp"
#include "wpi/math/controller/PIDController.hpp"
#include "wpi/math/controller/SimpleMotorFeedforward.hpp"
#include "wpi/units/angle.hpp"
#include "wpi/units/angular_velocity.hpp"

class Shooter : public wpi::cmd::SubsystemBase {
 public:
  Shooter();

  /**
   * Returns a command to shoot the balls currently stored in the robot. Spins
   * the shooter flywheel up to the specified setpoint, and then runs the feeder
   * motor.
   *
   * @param setpointRotationsPerSecond The desired shooter velocity
   */
  wpi::cmd::CommandPtr ShootCommand(wpi::units::turns_per_second_t setpoint);

 private:
  wpi::PWMSparkMax shooterMotor{ShooterConstants::kShooterMotorPort};
  wpi::PWMSparkMax feederMotor{ShooterConstants::kFeederMotorPort};

  wpi::Encoder shooterEncoder{ShooterConstants::kEncoderPorts[0],
                              ShooterConstants::kEncoderPorts[1],
                              ShooterConstants::kEncoderReversed};
  wpi::math::SimpleMotorFeedforward<wpi::units::radians> shooterFeedforward{
      ShooterConstants::kS, ShooterConstants::kV};
  wpi::math::PIDController shooterFeedback{ShooterConstants::kP, 0.0, 0.0};
};

C++ (Source)

#include "subsystems/Shooter.hpp"

#include "wpi/commands2/Commands.hpp"

Shooter::Shooter() {
  shooterFeedback.SetTolerance(ShooterConstants::kShooterTolerance.value());
  shooterEncoder.SetDistancePerPulse(
      ShooterConstants::kEncoderDistancePerPulse);

  SetDefaultCommand(RunOnce([this] {
                      shooterMotor.Disable();
                      feederMotor.Disable();
                    })
                        .AndThen(Run([] {}))
                        .WithName("Idle"));
}

wpi::cmd::CommandPtr Shooter::ShootCommand(
    wpi::units::turns_per_second_t setpoint) {
  return wpi::cmd::Parallel(
             // Run the shooter flywheel at the desired setpoint using
             // feedforward and feedback
             Run([this, setpoint] {
               shooterMotor.SetVoltage(
                   shooterFeedforward.Calculate(setpoint) +
                   wpi::units::volt_t(shooterFeedback.Calculate(
                       shooterEncoder.GetRate(), setpoint.value())));
             }),
             // Wait until the shooter has reached the setpoint, and then
             // run the feeder
             wpi::cmd::WaitUntil([this] {
               return shooterFeedback.AtSetpoint();
             }).AndThen([this] { feederMotor.SetThrottle(1.0); }))
      .WithName("Shoot");
}

A PIDController is declared inside the Shooter subsystem. It is used by ShootCommand alongside a feedforward to spin the shooter flywheel to the specified velocity. Once the PIDController reaches the specified velocity, the ShootCommand runs the feeder.