Skip to main content

Control Theory

What in the world is this?​

First, what is Control Theory anyways? Control Theory is the branch of engineering that deals with applying inputs to a system to get a specific output. If that sounds vague, it's because it is! The beauty of Control Theory is that it can apply to many different scenarios and systems. But that's not very useful when you're first learning, so let's look at a few specific examples of Control Theory in action:

  • A self-driving car: The computer applies inputs (throttle, brake, wheel position, etc) to the system (the car) to keep it on the road and drive to its destination.
  • A quadcopter: In fancier quadcopters, there is an onboard controller that can read from accelerometers and gyroscopes and automatically hold or go to specific positions.
  • An elevator: The elevator will apply force to the carriage in order to get it to the correct floor.

Closed-loop Control​

Closed-Loop Control is a fancy term that means that we are modifying the output to our system based on its input.

This is opposed to Open-loop control, which would a bunch of actions looking like this:

  1. Set robot drivetrain motors to max power
  2. Wait 10 seconds
  3. Turn off the robot drivetrain motors and hope you reached the desired destination.

Hint Hint, you probably didn't actually reach your destination

I believe an excellent way to think about closed-loop control is driving a car down a highway. In this example, your steering wheel doesn't need to be commanded, and there are no other cars on the road that you need to worry about avoiding. The only thing that you must do is keep as close to the speed limit (reference) as possible. Let's say you look at your car's speedometer and notice you are just below the speed limit. As a result of this observation, you press on the accelerator, and your car speeds up towards the reference. As you get closer to the reference, you back off on the accelerator more and more until your vehicle is at the reference. This concept is the general premise of Closed-Loop Control.

The reference is a term that represents where we want our system to be. A reference could be the target speed of a motor, the target angle that a drivetrain is facing, or any other state that we would like to control. This reference is the value that our closed-loop system is trying to converge our system's state to.

TLDR: The reference is what the controller tries to converge on.

Closed-loop control requires something to estimate or observe the state of a system. The overwhelming majority of the time in FRC is an encoder position/velocity, but it could easily be another sensor, such as a camera. This, unfortunately, means that closed-loop control has to be mildly more complicated than open-loop control.
This addition in complexity, though, is what allows closed-loop control to reject disturbances that would otherwise prevent your robot from properly achieving its purpose.

Bang-Bang Controller​

The Bang Bang controller is a type of closed-loop controller that abruptly switches between two or more outputs based on the current system state. While this controller can work in many situations, it is often suboptimal and can lead to undesirable oscillations.

Below is pseudocode that represents this behavior: 

reference = someValue;

while (referenceIsNotReached) {

    // obtain the encoder position
    encoderPosition = armMotor.getPosition();
    // calculate the error
    error = reference - encoderPosition;

    // if too low, move up, if too high then move down
    if (error > 0) {
        armMotor.set(-1);
    } else {
        armMotor.set(1);
    }

}

Be warned: simply checking if the controller is at its reference is a bad idea and will often result in an infinite loop occurring. This issue is because it is unlikely to arrive exactly at the reference. Users should set an error tolerance to account for this issue.

PID​

PID stands for proportional-integral-derivative. It's a form of closed loop control in order to compensate for inconsistencies in the motor and optimize when and how to reach and maintain the desired motor setpoints.

With the Bang-bang controller, the elevator would react to any error in the system's position. From the model below, we can see that a PID controller still consists of an output that's generated relative to the error between the reference and the system's state. If you are confused by the process, we'll break it down soon on this page.

Note: the motor image is not one of an FRC motor

In our team, we generally use custom PID with our Spark Max controllers from REV Robotics. Simply put, we tune the PID of these motors by plugging in the motors to our computer and opening the REV Client software. We then run the motor at a setpoint and adjust the various PID values in order to achieve the optimal gain and error values.

Here is an article explaining the detailed math behind PID. Or, you can read the below documentation!

It is worth noting that a PID controller can be used in different ways. Below are viable controllers that are variations of the PID Controller:

  • P Controller
  • PD Controller
  • PIDF Controller -> F term is a constant to counteract gravity

The Proportional Term​

The proportional term is arguably the essential part of the PID Controller. The proportional term is the part that does the majority of the lifting for most systems and is what drives the error the closest to 0. Increasing the proportional value (Kp) makes your system approach the reference faster, and decreasing it slows down the response. Naively, many may believe in increasing Kp as much as possible, but this will lead to many issues. Increasing Kp too high will result in what is called overshoot. Overshoot occurs when the controller cannot slow down the system quickly enough, and the system ends up moving past the reference before moving backward and settling back down. In many systems, such as that of elevators, this can be very dangerous.

Now, how do we properly tune Kp?

The recommended way to begin tuning with PID is to set I and D to 0 until you get the desired response from P. This logically makes sense because the proportional controller contributes to most of the controller's output most of the time.

Below is the response of a system with Kp = 1, Kd = 0, Ki = 0

The red line is the response of the system over time (such as the encoder position, angle of a potentiometer, IMU angle, etc) and the blue line is our reference signal. The yellow line is the command sent to the device we are attempting to control such as our motor.

As we can see from this example, we have a bit of steady-state error. The presence of steady-state error means that our controller is not tracking the reference well. We can likely combat this by increasing Kp. System response with Kp = 2, Kd = 0, Ki = 0

As we can see the performance of our system is improved. Notice that we are saturating our system slightly. The aforementioned is because our system can only actually go up to 1.0 power in WPILib. We are using more than this is not possible for our system. This issue is even more evident at higher gains.

The steady state error is slightly reduced but is still there. We will resolve this using the integral term

Note: for 99.9% of cases, Kp should be between 0.0 and 1.0.

The Integral Term​

The Integral term of a PID controller can be thought of as an extra bit of "kick" that the controller needs to converge on its reference. Integration is the process of adding up all of the parts into a whole. In this case, integration is the sum of all of the errors over time.

Integration is usually denoted by the symbol usually denotes integration '∫ '. This symbol is generally used to describe a function that gives the area under the line of another part. This may seem a little bit daunting to those without calculus knowledge, but trust me, it's straightforward in this context.

Traditionally in calculus, we are dealing with continuous functions such as f(x) = x^2. In FRC, we do not have this privilege. We are instead required to use discrete measurements. This means that we are getting new data at some intervals without having sizes in between. This, unfortunately, means that our integration will not be exact, and some approximations will be made. This can be done by using a Riemann sum (alternatively known as Forward Euler Integration). A Riemann sum can approximate the integral of the data by taking measurements at discrete points and then solving for the area as a rectangle. While this does tend to undershoot or overshoot the actual integral, the differences are pretty negligible for this application.

Integration is useful because it forces our error to be pushed toward 0 over time. This allows our system to be much more robust against disturbances such as gravity, friction, and backlash.

System response with Kp = 2, Kd = 0, Ki = 0.3

As we can see from the graph above, the integrator works to squash our error down to zero over time. This result, however, is still not quite perfect as we still have a large oscillation around our reference. The derivative term will resolve this issue.

The Derivative Term​

You can think of the PID controller's derivative term like a car's suspension or another type of dampener; it slows down oscillations and provides a smooth response. The derivative of a PID controller is just the current rate of change of the error. If you think back to your first algebra class, you probably remember a formula that looked something like the formula to get the slope of a line.

The derivative of a PID controller effectively does the same thing. We calculate the current rate of change of the error by doing the following: 

(error - last error) / delta time

The error is the difference between the reference and the current position. The last error is the recorded error of the previous loop, and delta time is the time between the sampling of the previous measurement and the current measurement.

The PID controller's derivative term affects the PID controller by subtracting from the output as the rate of change increases or decreases. Assuming that the controller is functioning correctly and error decreases, the derivative of the error will be negative. The result is then multiplied by its constant (Kd) and then added to the total output. A properly tuned derivative term will remove / significantly reduce oscillations of the controller.

Last time we had a controller with only Proportional and Integral control. This controller worked well to reach the reference with 0 steady-state error, but it suffers from violent oscillations. We can add a derivative to this system to attempt to reduce these oscillations.

System response with Kp = 2, Ki = 0.3, Kd = 0.2

This response is significantly better! It reaches 0 steady-state error, and it barely overshoots! With derivative, we need to be careful about how much Kd gain we add to the system. This is because the derivative slowing itself could cause a feedback loop and turn into an amplifier.

System response with Kp = 2, Ki = 0.3, Kd = 0.25

As you can see from this plot above, just a little bit of extra gain makes our system unstable, and eventually, it will grow into an uncontrollable oscillation. This effect is suboptimal, but as long as we properly tune our Kp, Ki, and Kd gains, this issue will not occur.

Order of Tuning a PID Controller​

To learn how to properly tune a PID controller, click here.

There are a few different ways to approach manually tuning a PID controller, but a popular one is to empirically tune the constants:

  1. Start with Kp, Ki, and Kd at 0.
  2. Increase Kp until steady-state error is very low.
  3. Increase Ki until steady-state error is removed entirely.
  4. Increase Kd until oscillations are removed.

This method above works well for many systems but many people have better luck with other methods.

Motion Profiling​

Why do we need motion profiling?​

Imagine that we're trying to move our robot along a 1D line to a certain reference position. If we wanted this movement to be precise, we could use a PID controller with our reference and current position. However, there is an issue. Unless the distance to our reference point is very small, our error at the start will be very large, which is going to cause a correspondingly large motor input.
For most FRC robots, applying maximum power from a standstill position will cause slip, which is undesirable because it results in rough movement, traction loss, and potential mechanical failure

Limiting Acceleration​

The trivial method to limit acceleration is to cap the output of the PID controller, but this has negative consequences towards system performance. We can do better.

What if we could directly choose a maximum acceleration? And a maximum deceleration too? (Slip also occurs when we decelerate too quickly!). What if we also wanted to specify a maximum velocity, because we may know that some velocities are too high to control reasonably?
That's where motion profiling comes in.

What are Motion Profiles?​

Motion profiles define a trajectory for our reference over a certain time period. For every time in this period, the motion profile tells what reference we should be at. The trajectory ends at our target reference. Essentially, motion profiles smoothly move our PID's reference point to limit acceleration and velocity. Motion profiling lets us move our robot and its mechanisms more smoothly and consistently.

To utilize a motion profile, we set our PID's reference point to whatever the motion profile tells us to, given the current time.

Trapezoidal Motion Profile​

The most common type of motion profile in FRC is the trapezoidal motion profile. It’s named that way because it results in a target velocity reference graph that looks like a trapezoid, since it is limitg acceleration and velocity.

It consists of three phrases: acceleration, cruise, and deceleration. In the first phase, the target velocity increases at the maximum acceleration. In the cruise phase, the target velocity doesn't change. Finally, the target velocity decreases by the maximum acceleration, ending at a target velocity of 0.

Trapezoidal motion profiles are relatively simple, and they are typically sufficient for smooth and precise motion for most mechanisms. To use a motion profile, you must use some controller, most commonly some variation of the PID controller.

Motion Profiles Conclusion​

One of the things to remember when doing motion profiling is that when you do get the PID parameters from the Robot charectarizer, you can program them into the Talon SRX motor controller instead of running the PID loop on the RoboRIO. Though this is much more complicated to do, it might yeild better performance. However, if and when you do motion profiling, getting it working should be the first priority, and only after you have properly working motion profiling do you focus on moving the PID processing to the Talon SRXs (Talon SRX is one of the advanced motor controllers that is connected through the CAN bus).