Applying Open- and Closed-loop Control in Your Code

Be sure you have read the previous page before heading here, as that is where all of the reasoning behind this code is from.

Applying PID Control

Applying PID control depends on what you're trying to do. However, once implemented it is pretty simple.

PID controllers, as well as Profiled PID Controllers (which include some feedforward constants) are both included as classes in WPILib.

A basic PID controller is an object that needs to be instantiated.

import edu.wpi.first.math.controller.PIDController;
PIDController controller = new PIDController(Kp, Ki, Kd);

The PID constants will stay the same and will need to be tuned to fit the specifications of the control system. For the system to function, a basic distance-time PID controller for an autonomous command is shown here:

    public MoveDistancePid(RomiDrivetrain drivetrain, double distance) {
        addRequirements(drivetrain);
        this.drivetrain = drivetrain;
        this.distance = distance;

        pid.setSetpoint(distance);
    }
    @Override
    public void execute() {
        double power = pid.calculate(drivetrain.getLeftDistanceInch());
        power = MathUtil.clamp(power, -1, 1);
        drivetrain.arcadeDrive(power, 0);
    }

There are a number of items to note here. Remember that in command based programming, all commands are checked every millisecond until they are finished. In this case, the PID uses the method pid.setSetpoint() in the constructor. However, this may not be the best practice in most practical application. The reason will be discussed later.

Secondly, since this is a distance-time PID controller, with the output being change over time, then we can insert the output directly in the system as power. Remember that what we insert into the motors has to always be power.

In this case, the all motion profiling is already hard-coded into the XRP. In addition, as the arm is servo-powered, adding an advanced control system isn't necessary.

For most intents and purposes, a closed-loop system is enough. However, in professional programming like on the competition robot, the following describes concepts that will become important.

Further reading: The PIDF Controller

PIDF stands for Proportional-Integral-Derivative-Feedforward. In the previous page, we saw the power of feedforward control algorithms in the context of closed-loop systems like PID. There isn't exactly a class in WPILib dedicated to this. However, the implementation is still rather simple.

import edu.wpi.first.math.controller.PIDController;
PIDController controller = new PIDController(Kp, Ki, Kd);
SimpleFeedforward feedforward = new SimpleFeedforward(Ks, Kv, Ka);

public MoveDistancePid(RomiDrivetrain drivetrain, 
                                double distance) {
        addRequirements(drivetrain);
        this.drivetrain = drivetrain;
        this.velocity = velocity;

        pid.setSetpoint(velocity);
    }
    @Override
    public void execute() {
        double acceleration = pid.calculate(
            drivetrain.getLeftDistanceInch()) + 
            feedforward.calculate(pid.getSetpoint());
        power += acceleration;
        power = MathUtil.clamp(power, -1, 1);
        drivetrain.arcadeDrive(power, 0);
    }

It is important to note that this type of controller is most often used in flywheels and basic drivetrains, of which the latter is the example shown. This was why it isn't necessarily ideal to put the setpoint in the constructor, it's that if the setpoint changes, you want it in a place that continuously updates it, like a method where commands are being processed. Here, the feedforward that is sent in is calculating how much feedforward is necessary here.

Further Reading: Motion Profiling

Motion profiling is important for precise control of the system. What it is, is creating a curve that dictates your exact velocity according to time.

For instance, here the system gains speed and looses speed at exact times. In its final form, the feedforward discussed earlier will bring the system to roughly the desired speed, and final corrections will be made with feedback control.

If your goal is to simply create a motion profile, then a PID can be a good option.

public void updatePIDs(double xaxisSpeed, double zaxisRotate) {
    
    leftVelocityPidController.setSetpoint(xaxisSpeed + zaxisRotate); //xaxisSpeed + zaxisRotate
    rightVelocityPidController.setSetpoint(xaxisSpeed - zaxisRotate); //xaxisSpeed - zaxisRotate

    leftAcceleration = leftVelocityPi dController.calculate(m_leftEncoder.getRate() * 0.05);
    leftSpeed += leftAcceleration;

    rightAcceleration = rightVelocityPidController.calculate(m_rightEncoder.getRate() * 0.05);
    rightSpeed += rightAcceleration;

    m_leftMotor.set(MathUtil.clamp(leftSpeed, -1, 1));
    m_rightMotor.set(MathUtil.clamp(rightSpeed, -1, 1));
  }

note that this method is built in the subsystem class

This is a basic motion profile where a PID controller will continuously update the velocity. However, the primary issue with this is that it's not what a PID is built for, therefore will have undesired effects. Here, the acceleration is painfully slow, because the velocity controllers are calculating the best path for the motors, not the motion profile.

Adding feedforward would improve the situation. Take this flywheel:

private void updatePIDs() {
        double upperError = Math.abs(upperController.getSetpoint() - encoderUpper.getVelocity());
        double lowerError = Math.abs(lowerController.getSetpoint() - encoderLower.getVelocity());

        // Count how many loops the RPM of the motors is in a good range - used to know when velocity is stable
        if (upperError < 20 && lowerError < 20) {
            atSpeedCounter++;
        } else {
            atSpeedCounter = 0;
        }

        upperPower = upperController.getSetpoint() * upperKv;
        lowerPower = lowerController.getSetpoint() * lowerKv;

        // Only run PID controller when within 300 RPM of target - feedforward is enough until that point
        if (upperError < 300) {
            upperPower += upperController.calculate(encoderUpper.getVelocity());
        }
        if (lowerError < 300) {
            lowerPower += lowerController.calculate(encoderLower.getVelocity());
        }

        upperPower = MathUtil.clamp(upperPower, -12, 12);
        lowerPower = MathUtil.clamp(lowerPower, -12, 12);

        motorUpper.setVoltage(upperPower);
        motorLower.setVoltage(lowerPower);
    }

    /**
     * Sets the target velocities in RPM of the Launcher motors
     * @param upper The desired RPM of the top motor
     * @param lower The desired RPM of the bottom motor
     */
    public void setVelocities(double upper, double lower) {
        upperController.setSetpoint(upper);
        lowerController.setSetpoint(lower);
    }
    public void setPowers(double upper, double lower) {
        motorUpper.set(upper);
        motorLower.set(lower);
        noPidTimer.restart();
    }

The feedforward will first set a value, which the motor will attempt to target, after which the feedback controllers will correct the rest of the error.

For a real motion profile, WPILib has a real class for this. Enter the Profiled PID Controller. This is a PID controller with a few more parameters: a maximum acceleration and a maximum velocity. With these upper limits, our motion profile will be closer to what is desired.