# CTRL ALT FTC
FIRST Tech Challenge Team #22377 The Sigmacorns presents CTRL ALT FTC — the most comprehensive Control Theory guide for \*FIRST\* Tech Challenge Teams.
{% hint style="success" %}
Returning visitors: we are currently looking for ways to adapt in order to best assist the needs of the *FIRST* Tech Challenge community. If you have, time it would mean the world to us if you filled out this form. Any and all feedback is appreciated and can be quickly [left here](https://docs.google.com/forms/d/e/1FAIpQLScnAANnEH6ahHE7eQSAwdwcxpJuJWk-fX8zdBFKi6WB80BJfQ/viewform).
{% endhint %}
### Control System Example
{% embed url="" %}
Example of feedback control with the Pure Pursuit algorithm
{% endembed %}
## What and Who is this Guide For?
This article is an introductory guide to the beautiful world of Control Theory for those with a background in FTC programming. This guide is for those who have just learned the basics of programming but are stuck in the grey "middle area" where resources are far below your abilities or just a little above your head. Hopefully, by the end of this guide, you will have a more intuitive understanding of control topics and feel more comfortable with the implementation of advanced controls in the context of *FIRST* Tech Challenge and other fields as your STEM journey continues.
This guide assumes that you have experience with the SDK. While this does make it slightly tricky for absolute beginners to follow, it also allows the knowledge to be applicable outside of *FIRST* Tech Challenge.
***Note: all of the code examples are pseudocode of how you would go about a Java implementation. Please understand that there may be a little more work to do than what is shown in the document, as these extra details would take away from the learning experience in many cases.***
If you would like to contribute information to CTRL ALT FTC, you can access the [GitHub](https://github.com/BenCaunt/CTRL-ALT-FTC) and make a pull request.
Thanks to everyone who has contributed! [](https://github.com/BenCaunt/CTRL-ALT-FTC/graphs/contributors)
Made with [contrib.rocks](https://contrib.rocks).
# Why Control Theory is A Must Have
What are control systems, and why do we use them?
## Why Controls?
*Control theory* is a field of engineering that deals with designing controllers (typically implemented in software) to move a system to the desired state. Some systems are easy to control, some are difficult to control, and some are even impossible to control. Knowing which systems to design for and implementing a controller for them is a crucial skill that proves useful in many real-world jobs. It enables your robot to perform more reliably and efficiently. Implementing robust controls in your robot not only improves autonomous performance but also provides the peace of mind that your robot will behave as expected.
# Introduction to Open Loop Control
Introduction to open loop control in FTC
## What is Open Loop Control?
Open loop control simply means giving a specific output without checking it based on our input. A basic example of this is that of a time-based autonomous:
```java
// Start motors at desired speed
frontLeft.setPower(1);
frontRight.setPower(1);
backLeft.setPower(1);
backRight.setPower(1);
// Leave motors running for desired time
sleep(500)
// Stop motors after desired time
frontLeft.setPower(0);
frontRight.setPower(0);
backLeft.setPower(0);
backRight.setPower(0);
```
Another example of open loop control is that of an intake which runs continuously:
```java
// set intake power at max output
intakeMotor.setPower(1);
```
{% hint style="info" %}
These examples and nearly all later examples assume that you have correctly set up prerequisite code such as initializing motors. If this has not been done yet, I would recommend setting this up before proceeding.
{% endhint %}
As you can see, open loop control is an incredibly simple way to get your robot performing actions. Unfortunately, this method has a few critical downfalls for certain systems:
* Cannot reject disturbances
* Cannot overcome contact from other robots
* Energy inefficient
* Motors must run at maximum power to utilize their maximum torque potential
* Consistency will vary as battery voltage changes.
These downfalls really only matter for certain types of systems. For example, your intake doesn't really need to reject disturbances from the outside world like your drivetrain does. This means that choosing Open Loop Control vs Closed Loop Control is decided with personal preference and the ***design requirements*** of your system.
{% hint style="info" %}
**Design Requirements**: What your system needs to accomplish.
Does it need to reject disturbances? Does it need to be fast? Can it be slow? How accurate does it need to be? All of these things are factored into your system's ***design requirements***.
{% endhint %}
#### Practice Exercises
* Try running the first example on your team's drivetrain
* How consistent is it?
* Does the distance vary with the battery voltage?
* Try to think of how to improve upon this method and implement those to see if it works!
# Introduction to Closed Loop Control
Introduction to feedback control in FTC
## What is Closed Loop Control?
Closed-Loop Control means that we are **modifying our input to our system based on its output.**
I believe an excellent way to think about closed-loop control is driving a car driving 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 of the accelerator more and more until your vehicle is at the reference. This concept is the general premise of Closed-Loop Control.
{% hint style="info" %}
The **reference** is a term that 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 on.
**TLDR:** The reference is what the controller tries to converge on.
{% endhint %}
Closed-loop control requires something to estimate or observe the state of a system. The overwhelming majority of the time in FTC is an encoder, but it could easily be another sensor, such as a rev distance sensor. This observer, 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.
Here we can see two types of closed-loop controllers.
```java
/*
* Bang Bang Controller
*/
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.setPower(-1);
} else {
armMotor.setPower(1);
}
}
```
```java
/*
* Proportional Controller
*/
reference = someValue;
while (referenceIsNotReached) {
// obtain the encoder position
encoderPosition = armMotor.getPosition();
// calculate the error
error = reference - encoderPosition;
// set motor power proportional to the error
armMotor.setPower(error);
}
```
{% hint style="danger" %}
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. However, the robot will come very close, and so a structure more like the following will work better:
{% endhint %}
```java
/*
* safer exit condition
*/
while (Math.abs(error) > tolerance) {
// perform controller operations
}
```
In the above examples, we have two very different types of controllers. The first one is what the average programmer would likely develop as a solution to this type of problem. This is called the 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.
The second controller may look a little less familiar. This controller is the proportional feedback controller. This type of controller generates some output directly proportional to the **error** between the reference and the current state. The result of this controller is that the output is much smoother and much more predictable. The proportional feedback controller structure is almost always more desirable than that of a bang-bang controller due to the proportional controller's continuous and linear nature. We can also add onto this controller and create more complex items such as the PID controller.
The consensus is that the proportional controller is superior to the bang-bang controller. This, however, is still not recommended, and it is often beneficial to move towards a variant of the proportional controller known as the Proportional Integral Derivative (PID) controller.
In the next chapter, we will begin to learn about this new closed-loop controller.
#### Practice Exercises
* Try these two controllers on a drivetrain
* your reference should be some value in encoder ticks you want your robot to travel.
* Does using the same controller for each of the 4 motors work or should each motors output be calculated seperately?
* Try both!
* Which controller performs better, the bang bang controller or the proportional controller.
* Do the results suprise you?
* Do the results contradict this article? (stay tuned for the next chapter for further advancements).
# The PID Controller
From the Proportional Controller to the PID Controller.
## What is a PID Controller?
A PID Controller has its roots tied to the proportional controller we looked at in the previous chapter but has numerous additions. The structure is, however, very similar. From the Simulink model below, we can see that it still consists of some output that's generated relative to the error between the reference and the system's state.
What we have changed is what is inbetween the error and our system.
Our PID Controller consists of three main parts. The Proportional (P), the Integral (I), and the Derivative (D).
{% hint style="info" %}
There are a few things in this model above that may not be totally intuitive at first glance.\
\
1\. The triangle symbols signfiy a "gain" or a value multiplied by its input.
2\. The block with the three addition symbols adds up the entirety of its three inputs as its output.
{% endhint %}
### What do the constants "Kp, Ki, and Kd" do?
PID has three values that the programmer tunes. These values are Kp, Ki, and Kd. These values are multiplied by their corresponding input. Changing these values changes how the controller behaves. The proportional term or **Kp** is a value that is directly proportional to the error of the system. To get the Proportional output, we take the **Kp \* error** and add it to our system's input. Changing Kp determines how fast or how slow the system moves towards 0 error. **You can think of Kp like a rubber band, where the thicker it is, the harder it is to pull away from its equilibrium point.**
The derivative term or **Kd** is a value directly proportional to the rate of change of the error of the system. To calculate this, we can find the slope of the error from the last update to the current update of the loop. Derivative ensures that our system isn't responding too quickly by penalizing excessive rates of change. **You can think of Kd as increasing the friction of your skates on an ice skating rink. The lower the friction, the harder to control, and you need a little bit of \_dampening**\_\*\* to allow you to keep yourself stable.\*\*
The Integral term or **Ki** is directly proportional to the **sum of all the errors over time**. This allows the system to **overcome** nonlinear effects such as static **friction** that would otherwise be difficult to tune out. **This works by adding up the remaining error until the output grows large enough to overcome the constant disturbance.**
## Pseudocode Implementation of PID
```java
/*
* Proportional Integral Derivative Controller
*/
Kp = someValue;
Ki = someValue;
Kd = someValue;
reference = someValue;
integralSum = 0;
lastError = 0;
// Elapsed timer class from SDK, please use it, it's epic
ElapsedTime timer = new ElapsedTime();
while (setPointIsNotReached) {
// obtain the encoder position
encoderPosition = armMotor.getPosition();
// calculate the error
error = reference - encoderPosition;
// rate of change of the error
derivative = (error - lastError) / timer.seconds();
// sum of all error over time
integralSum = integralSum + (error * timer.seconds());
out = (Kp * error) + (Ki * integralSum) + (Kd * derivative);
armMotor.setPower(out);
lastError = error;
// reset the timer for next time
timer.reset();
}
```
# The Proportional Term
More information on the proportional term of t he PID Controller.
## 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 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 linear slides, this can be **very dangerous**.
### How does changing Kp affect the dynamics of our system?
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 inherently makes sense because the proportional controller contributes to most of the controller's output most of the time.
In the model above we have our familiar PID controller but with our integral and our derivative disabled (set to 0). This means that only our proportional control is active. With Kp set to 1 this system should act Identically to the proportional controller in the [Introduction to Closed Loop Control ](https://www.ctrlaltftc.com/introduction-to-closed-loop-control)chapter.
Here we can see the response from this system:
{% hint style="info" %}
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.
{% endhint %}
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.
As we can see the performance of our system is improved. Do notice that we are **saturating** our system slightly. The aforementioned is because our system can only actually go up to **1** power in the FTC SDK. We are using more than this is not possible for our system. This issue is even more evident at higher gains.
We can see here that the steady state error is slightly reduced but is still there. We will resolve this in the next chapter.
#### Practice Exercises
* Try implementing a P controller on a simple motor
* Try controlling the encoder position of the motor
* Try controlling the speed of the motor
* The DcMotorEx class has a getVelocity method that you can use to get the number of encoder ticks per second
* What happens if the gain Kp is too low?
* What happens if the gain Kp is too high?
* Try implementing the controller on a drivetrain
* Does it work better if each motor has it's own controller?
* Does it work better if all the motor uses the same controller?
# The Integral Term
Introduction to the integral term
## 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.
{% hint style="info" %}
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.
{% endhint %}
Traditionally in calculus, we are dealing with ***continuous*** functions such as f(x) = x^2. In FTC, 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.
## How do we Integrate discrete data?
Integration of discrete data is inherently very different than that of continuous systems. This is not necessarily a bad thing for us. While it means that we have to make some approximations that will lead to slight inaccuracies, this doesn't matter too much for PID. The most common method of integration of discrete systems is known as "forward Euler integration".
\
Forward Euler Integration works by the following sequence:
* Measure the data at the current time step.
* Calculate the time since we last recorded data.
* Multiply the data by the time since we last recorded data.
* Add the product to a running total containing the integral sum.
* repeat until we are out of data (or keep repeating forever, whatever is necessary).
{% hint style="success" %}
For you calculus-inclined individuals, Forward Euler Integration may already be familiar to you as Riemann sum.
{% endhint %}
As you can see from the figure above, Forward Euler Integration 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 authentic integral, the differences are pretty negligible for this application.
### Software implementation
We actually implemented forward Euler integration already in the Software implementation of PID in our introduction to PID.
```
integralSum = integralSum + (error * timer.seconds());
```
This integral sum is then added to the output of our controller which is then sent as the input to our system.
## Effect of Integration in PID
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.
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 next chapter will resolve this issue.\
\\
# The Derivative Term
Introduction to the Derivative term
## 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 following 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 previous error, and delta time is the time between the sampling of the previous measurement and the current measurement.
```java
derivative = (error - lastError) / timer.seconds();
```
### How does Derivative effect the PID controller?
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.
### Picking up where we left of
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 derivative to this system to attempt to reduce these oscillations.
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.
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.
In the next chapter, we will look at different methods to tune a PID controller to achieve optimal results.
### Practice Exercises
Now that we fully understand each of the three terms of a PID controller, implement the controller on a drivetrain or some other mechanism and try your best at tuning it.
# Tuning Methods of A PID Controller
Now that we understand the fundementals of a PID controller, how do we tune it?
## Tuning Methods
#### Manual Tuning
There are a few different ways to approach manually tuning a PID controller, but a popular one appears to be the following:
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.
Manning PID controllers manually require a little bit of experience, but this knowledge is rudimentary to learn.\
\
This is a table from the Wikipedia article on PID controllers that characterize how an increase in each term affects the system to get you started.
### Finding reasonable starting values for Kp
Finding a good value for Kp to begin with can be very helpful. To do this you must know the maximum output of your system and the units that you are using to measure your system.
#### Effect of each term on the controller's performance
| Parameter | Rise time | Overshoot | Settling time | Steady-state error | Stability |
| --------- | ------------- | --------- | ------------- | ----------------------- | ---------------------------- |
| Kp | Decrease | Increase | Small change | Decrease | Degrade |
| Ki | Decrease | Increase | Increase | Eliminate | Degrade |
| Kd | Little change | Decrease | Decrease | Theoretically no change | Improve if Kd is already low |
Table information sourced from:
{% hint style="info" %}
Ziegler-Nichols is a method that can yield mixed results, you must be careful not to break your system.
{% endhint %}
#### Ziegler–Nichols Tuning
Ziegler Nichols tuning is a method that is similar to manual tuning in the fact that it requires physical access to the System. Still, it is much more rigorous and, in many situations, can provide superior results. This method requires that you visualize your error over time in some way. There are a few ways to do this. One is to use the [FTC dashboard](https://acmerobotics.github.io/ftc-dashboard/), and another is to use System.out.println() and then copy and paste the data from logcat into excel (suboptimal, I'm aware, but it's a quick fix to get something working).
The Ziegler Nichols procedure works as the following:
1. Start with Kp, Ki, and Kd at 0.
2. Increase Kp until the system is in a steady oscillation around the setpoint.
1. This is known as the critical gain of the system and is denoted as **Ku**.
3. The oscillation period or the time from peak to peak of the oscillation is a value known as **Tu**.
4. We then use the following lookup table to derive our PID gains.
| Desired Controller | Kp | Ki | Kd |
| ------------------ | ------ | --------- | --------- |
| P Only | 0.5Ku | N/A | N/A |
| PI Only | 0.45Ku | 0.54Ku/Tu | N/A |
| PD Only | 0.8Ku | N/A | 0.1KuTu |
| PID | 0.6Ku | 1.2Ku/Tu | 0.075KuTu |
| Some overshoot | 0.33Ku | 0.66Ku/Tu | 0.11KuTu |
| No overshoot | 0.2Ku | 0.40Ku/Tu | 0.066KuTu |
Table sourced from:
After following the procedure above, you should theoretically have well-tuned gains that perform as expected depending on the given tuning requirements you chose. However, many do claim that often Ziegler Nichols tuning requires a little bit of fine-tuning to get perfect.
####
# Improvements to the PID Controller
Discussion of methods to improve the PID controller such as anti-integral windup, low-pass filter derivative, and gain scheduling.
## Issues with the Traditional PID Controller
The traditional PID implementation as seen in previous chapters has a few inherent issues. The two most common ones which we will discuss are that of integral windup and derivative noise amplification.
Each one of these methods has a relatively basic solution which we will analyze as this chapter progresses.
#### Basic problems and solutions
* Integral windup
* Integral sum cap
* Integral sum reset
* Stop integral sum when the output is being saturated.
* Derivative noise amplification
* Filter derivative input
### Integral Windup and Mitigation Methods
Integral windup is a phenomenon that occurs whenever the integral output saturates our system. Integral windup causes the system to remain traveling in the same direction for some time until the integral sum drops low enough for our system to regain control. Brian Douglas does a fantastic job of explaining the issue of Integral windup on the Matlab youtube channel [here](https://youtu.be/NVLXCwc8HzM?t=201).\
\
There are a few easy things that can be implemented to help reduce the likely hood of integral windup occurring. One of these is to simply put limits on our Integral sum such as in the following code example:
```java
// sum our integral
integralSum = integralSum + (error * timer.seconds());
// set a limit on our integral sum
if (integralSum > integralSumLimit) {
integralSum = integralSumLimit;
}
if (integralSum < -integralSumLimit) {
integralSum = -integralSumLimit;
}
```
The code above effective sets hard limits on how big our integral sum can arrive at. For FTC motor control I recommend making it so that your integralSumLimit \* Ki is around \~0.25. This is definitely up to preference and will need to be played around with a bit but it is enough to where it actually makes a difference in most systems but not too much that the system can become unstable.
Another thing that is good practice to do for many systems is to reset our integral sum whenever the reference changes.
{% hint style="warning" %}
Integral reset is a technique that needs to be evaluated on a system-by-system basis. It will inherently play better with some systems than others.
{% endhint %}
Here is how to implement the integral reset in software:
```java
// reset the integral if the reference is changed.
if (reference != lastReference) {
integralSum = 0;
}
```
For many systems such as a drivetrain, doing this allows you to more easily change directions without waiting for the integral sum to change directions.
### Derivative Noise Mitigation Methods
If we recall from the chapter on the derivative term of a PID controller we know that increasing the gain of our derivative term can potentially result in unstable oscillations. This is because the nature of the derivative when it attempts to slow down the rate of change of the system can create an unstable feedback loop resulting in oscillations that increase in amplitude. The same thing can happen when our source of data is unreliable and noisy. While we cannot perfectly fix noisy data without a perfect model we can use a series of filters to remove much of the high-frequency noise that appears in our measurements. One such method is known as the **low pass filter**.\
\\
In the above graph we can see how the low pass filter is able to remove significant amounts of the noise of our measurement but how does it do this?
The low pass filter takes the following form:
$$
x\_c=ax\_p+(1-a)x\_m
$$
Where:
**Xc** = current estimate\
**Xp** = previous estimate\
**Xm** = current measurement\
**a** = measurement gain (0 < a < 1)
This filter is tuned by adjusting the gain **a**. Small values of **a** allow each new measurement to have more influence on the estimate than small values of **a**. This filter works because we are calculating the **previous estimate \* the percentage + the measurement \* the complement of the percentage** **(1 - a)** which results in a whole estimate being created. This process iterates, updating the estimate at each timestep.
#### Low Pass Filter Implementation
The low pass filter can be implemented in software similar to the following example:
```java
a = 0.8; // a can be anything from 0 < a < 1
previousEstimate = 0;
currentEstimate = 0;
currentEstimate = (a * previousEstimate) + (1-a) * error
previousEstimate = currentEstimate
```
The following code if used as the input to our derivative will likely have significantly improved performance with the use of noisy sensors. These noisy sensors may include but are certainly not limited to the Analog Gyroscopes and the Distance sensors. These sensors produce high-frequency noise that has the potential to cause issues if not properly filtered.\
\
The end result of our additions to both our Integral and Derivative terms looks something like the following:
```java
/*
* Proportional Integral Derivative Controller w/ Low pass filter and anti-windup
*/
Kp = someValue;
Ki = someValue;
Kd = someValue;
reference = someValue;
lastReference = reference;
integralSum = 0;
lastError = 0;
maxIntegralSum = someValue;
a = 0.8; // a can be anything from 0 < a < 1
previousFilterEstimate = 0;
currentFilterEstimate = 0;
// Elapsed timer class from SDK, please use it, it's epic
ElapsedTime timer = new ElapsedTime();
while (setPointIsNotReached) {
// obtain the encoder position
encoderPosition = armMotor.getPosition();
// calculate the error
error = reference - encoderPosition;
errorChange = (error - lastError)
// filter out hight frequency noise to increase derivative performance
currentFilterEstimate = (a * previousFilterEstimate) + (1-a) * errorChange;
previousFilterEstimate = currentFilterEstimate;
// rate of change of the error
derivative = currentFilterEstimate / timer.seconds();
// sum of all error over time
integralSum = integralSum + (error * timer.seconds());
// max out integral sum
if (integralSum > maxIntegralSum) {
integralSum = maxIntegralSum;
}
if (integralSum < -maxIntegralSum) {
integralSum = -maxIntegralSum;
}
// reset integral sum upon setpoint changes
if (reference != lastReference) {
integralSum = 0;
}
out = (Kp * error) + (Ki * integralSum) + (Kd * derivative);
armMotor.setPower(out);
lastError = error;
lastReference = reference;
// reset the timer for next time
timer.reset();
}
```
Now we have fixed any of the issues that can cause issues with your control system. We have fixed the issue of derivative amplifying noise in the system and the issue of integral windup. Now your system will likely be more stable and there is significantly less risk of external disturbances or poor sensor quality disrupting your robot on the field.
# Feedforward Control
Re-opening the loop
In the previous chapters, we focused on methods of feedback control. Feedback control is excellent for many systems and allows for effects such as disturbance rejection to improve the system's performance. While this approach is exemplary, we can often increase performance even further with the addition of an open-loop feedforward controller.
Feedforward works by essentially 'predicting' the mapping between plant inputs and outputs rather than iteratively converging on the desired result with a feedback controller.
In general, the advantage of feedforward (or open-loop) control is that it is immune to both time delay and sensor noise. This means that, unlike feedback control, bad sensor data or delays between inputs are much less likely to cause a system to be unstable.
Another great use for feedforward is to replace the integral term of a feedback controller. Using feedback as an alternative to specifically the integral term is often a valid solution if one is worried about issues such as integral windup impacting the system's performance.
### Software implementation of feedforward
```java
double output = K * reference;
```
As you can see, feedforward is more trivial compared to the feedback control loops such as PID that we implemented in previous chapters. This is because feedforward is acting almost as a unit conversion between some input and some output. This conversion is determined by the tuned value `K` in the code snippet above.
In FTC this value is often referred to as `Kv`, specifically when dealing with controlling the velocity of a plant. Additionally, a term for acceleration can also be added, commonly referred to as `Ka`.
```java
double output = (Kv * referenceVelocity) + (Ka * referenceAcceleration);
```
Using acceleration and velocity references together for our output allows our system to follow trajectories. Trajectories effectively tell our system at any given point in time what the system's states should be.
### Closing the loop
We would often like to have the benefits such as improved disturbance rejection that feedback control offers while keeping the stability that feedforward control provides. We can achieve this by coupling the outputs together.
Here we can see a proportional and feedforward controller for a velocity system:
```java
while (setPointIsNotReached) {
double output = Kp * (referenceVelocity - currentVelocity) + Kv * (referenceVelocity);
}
```
We can also use feedforward in combination with a full PID controller such as the one we developed in previous chapters. Often, this setup can be referred to as PIDF:
```java
while (setPointIsNotReached) {
double output = PID(referenceVelocity, currentVelocity) + Kv * (referenceVelocity) + Ka * referenceAcceleration;
}
```
### Arm Feedforward
Rotating arms such as the one in the image below can be very easy to make, but can be very challenging to program. This is because of the nonlinear dynamics caused by gravity as the arm rotates. In order to compensate for these nonlinear dynamics, we can use a gravity feedforward to linearize the system.
To compensate for the force of gravity, we take the cosine of our target angle relative to the ground, as shown in the following example.
```java
double Kcos = SOME_VALUE_YOU_TUNE;
double reference = YOUR_TARGET_ANGLE;
double power = Math.cos(reference) * Kcos;
```
We can then use the code above with a PID controller or another type of feedback controller. Using this will improve the reliability of the control system, as it will more accurately compensate gravity than a simple integrator. However, we recommend using both this and an integrator for optimal results.
This works because `cos()` represents the ratio between the adjacent side of a right triangle and the hypotenuse of the triangle. Whenever the arm is extended straight out (0 degrees), the value of the cosine function is at its maximum (1). This is the point where the system demands the most torque to hold its weight. Whenever we want the arm to be oriented straight up or down (90 degrees), the arm does not require any torque to hold its weight. This matches our feedforward controller, as `cos(90 degrees)` is 0.
By using a nonlinear feedforward controller, we can improve the reliability of our control system and improve the performance of our system.
### Slide Gravity Feedforward
For cascading linear slides, the force of gravity is generally nearly constant. For continuously rigged linear slides, this assumption is not as true, but the difference is generally negligble, so it can be ignored.
In physics, the force of gravity is equal to the mass of a system \* the gravitational constant `g`.
$$
f=mg
$$
Combined with Newton's Second Law:
$$
f=ma
$$
We can conclude that the acceleration due to gravity is constant:
$$
ma=mg \ a = g
$$
In order to compensate for this, we can simply tune a constant power to reject the disturbance:
```java
double Kg = // tune till the slide holds itself in place.
/*
* PID for general movement of the system
* The feedforward removes the disturbance
* This improves the responsiveness of the PID controller
*/
double output = PID(reference, state) + Kg;
```
# Full State Feedback
A modern control theory approach to feedback control
If you recall from our [introduction to closed loop control](/introduction-to-closed-loop-control) and the [proportional controller](/the-pid-controller/the-proportional-term) from earlier, we can roughly control a systems state using the formula:
$$
u(t)=k(r(t)-x(t))
$$
Where:\
`u(t)` is our current motor input;\
`x(t)` is our current system state; and,\
`r(t)` is our current reference
Full state feedback takes a slightly different approach than PID control, but has the same idea.
One can model a linear system in the form:
$$
\dot{x}=Ax+Bu
$$
where x is the vector that represents the state of your system.
A simple model will have two states in this vector: position and velocity.
$$
\begin{bmatrix} x \ \dot{x} \end{bmatrix}
$$
We want to then move these values to some desired state. such as:
$$
\begin{bmatrix} 0 \ 0 \end{bmatrix}
$$
We can then perform feedback on this vector directly if we have some gain vector `K`.
$$
\begin{bmatrix} k\_1 \ k\_2 \end{bmatrix}
$$
We want to move `x` to our desired state of zero. We can perform feedback by first subtracting the observed state from the desired state and then multiplying this vector by the gain `K`, just as we would for a traditional feedback controller such as PID.
$$
\begin{bmatrix} 0 \ 0 \end{bmatrix}
-\begin{bmatrix} x \ \dot{x} \end{bmatrix}
\= - \begin{bmatrix} x \ \dot{x} \end{bmatrix}
$$
We can then compute the dot product between this result and `K`.
$$
\begin{bmatrix} k\_1 \ k\_2 \end{bmatrix} \cdot - \begin{bmatrix} x \ \dot{x} \end{bmatrix}=-k\_1x-k\_2\dot{x}
$$
So, our control input is defined as:
$$
u(t)=-k\_1x-k\_2\dot{x}
$$
This control law will stabilize `x` and `x-dot` to the state of zero but in many systems we want to go to a state other than zero. We can generalize this control law for any desired state by the final control law:
$$
\begin{bmatrix}x\_r-x\ :::\dot{x\_r}-:\dot{x}\end{bmatrix}\begin{bmatrix}k\_1\ :k\_2\end{bmatrix}=k\_1(x\_r-x)+k\_2(\dot{x\_r}-\dot{x})
$$
What we have now is effectively just a proportional and derivative controller. What makes this different is that at each point in time, our target velocity is not necessarily zero. As a result, this type of controller will have much better performance following trajectories such as motion profiles and will not rely as much on feedforward control to get good transient performance like a PID controller would (though feedforward is always nice to have!).
What makes this controller even more interesting is when you add extra states. If you were to use a Kalman filter or some other filter to observe the acceleration of your system you could have even more accurate trajectory following performance. This also goes the other way, adding a state that is the integral of position will do the same as a PID controller.
Full state feedback control allows us to control each relevant state of our system. The formula above shows that we are beginning with our primary state `x_1` and couple the outputs with each other state of our system. An example of what these states could be is position, velocity, etc.
```java
/**
* Calculate state feedback for position and velocity of our system.
*/
public double calculateStateFeedback(double targetPosition, double targetVelocity) {
double positionError = targetPosition - robotPosition;
double velocityError = targetVelocity - robotVelocity;
double u = (positionError * k1) + (velocityError * k2);
return u;
}
```
This type of controller will have much better trajectory tracking performance than PID as in addition to driving our robot to the target position, it is able to drive our robot at target velocity. In combination with motion profiling, this is an incredibly powerful tool. You can even extend this method by adding additional states. Combined with feedforward control, you will have an incredibly robust controller.
# The Kalman Filter
State observation is a crucial component of control theory but is often overlooked in FTC
## Why Kalman Filter?
A Kalman filter at the highest level is an algorithm that optimally estimates any given state of a system, given a model of how the system changes over time and knowing a set of sensor measurements. We use a Kalman filter whenever we have doubts about the quality of our sensors, and we require more reliable measurements to control our system with the performance that we desire.
Imagine trying to move your robot from one point to another. There are two general ways you can approach this. One way would be to model a path that should theoretically take your robot to the point (for example, a motion profile). The other would be to move your robot until a sensor tells you you’ve gotten to the point. Both of these approaches have drawbacks: a modeled path will never be perfect as there are always factors you can’t account for, and a sensor may have drift and/or unreliable measurements. The magic of the Kalman filter is that it combines these methods, allowing for a lessening of the impacts of these drawbacks.
The Kalman filter uses the robot’s current state (ex: position, velocity) to predict where it will be in the future. This prediction can vary in complexity, from comprehensive models with many variables to much simpler models. Then, when a new sensor measurement comes in, it updates this prediction. By blending the predictions and measurements you obtain a more accurate estimate than you could get from just the predictions or the measurements alone.
## How does the Kalman Filter work?
A quick disclaimer: this page is intended to build enough intuition about the Kalman filter to allow you to implement a Kalman filter for FTC applications yourself. As such, many simplications have been taken and this explanation is by no means mathematically rigorous.
Now, let’s start with how the Kalman Filter keeps track of the variables we want it to estimate. For a given variable, such as a robot’s angle, we have a value that is associated with that variable (ex: $$15$$ degrees). However, in addition we also want to know how much uncertainty exists in that variable. The way the Kalman filter assigns uncertainty to a variable is through the metric of variance, with a high variance corresponding to high uncertainty.
With that, we can begin to understand how the Kalman filter works. We will begin with the prediction step:
$$x\_t=x\_{t-1}+u\_t$$
In this equation, $$x$$ represents the value of the variable we want to track and u represents how much we believe the value will change. You will notice that $$x$$ has subscript which denotes the time step, with t-1 meaning the previous time step and t being the current time step. Putting this all together, this equation shows that our predicted value is the sum of our previously estimated value and the amount we believe the value will change. The value of $$u\_t$$ is something you will need to determine yourself how to calculate (ex. Change in position from odometry for velocity).
When we make a prediction, we are also changing the uncertainty we have in that variable. This takes form in the following equation:
$$p\_t = p\_{t-1} + q$$
Here, $$p$$ represents the variance of $$x$$ and $$q$$ represents the variance of our model. When we use our model, we always add uncertainty to our estimate because we can’t be certain our model is correct. The amount of uncertainty in our model, $$q$$, is something that you will have to tune to your system.
Now that we have a prediction for what our variable will be, we will combine it with our sensor measurement. We update our value using this equation:
$$x\_t = x\_{t-1}+K\_t(z\_t-x\_t)$$
In this equation, $$z\_t$$ is the value of our measurement and Kt is a constant between $$0$$ and $$1$$ that we will later define. The expression $$(z\_t-x\_t)$$ is the difference from the new measurement we just made and what we have already estimated the state to be. That is being multiplied by $$K\_t$$, which is also called the Kalman gain. This number dictates the influence of the measurement in the estimate. We will see how this number is calculated next, but notice that if $$K\_t=1$$, then $$x\_t=z\_t$$ and if $$K\_t=0$$, $$x\_t=x\_t$$. The closer $$K\_t$$ is to $$1$$, the more trust we put in the measurement, and the closer it is to $$0$$, the more trust we put in our model.
Let’s now compute the Kalman gain as follows:
$$K\_t = \frac{p\_t}{p\_t+r}$$
$$r$$ is the variance in our measurement. You may notice that this formula is a simple proportion, and that makes a lot of sense. If $$p\_t$$ is high compared to $$r$$, $$K\_t$$ will have a value close to $$1$$. This makes sense because if there is more uncertainty in our model than our measurement we should weigh the measurement more, which a $$K\_t$$ value close to $$1$$ achieves. In the opposite case, when $$r$$ is high compared to $$p\_t$$ then $$K\_t$$ has a value close to $$0$$, favoring the model more than the measurement.
Alas, when we update our estimated value we must also update its uncertainty.
$$p\_t=(1-K\_t)p\_t$$
Since $$K\_t$$ ranges from $$0$$ to $$1$$, $$(1-K\_t)$$ will also range from $$0$$ and $$1$$, so our measurement will always decrease uncertainty in our estimate. When $$K\_t$$ is close to $$0$$, we do not factor in the measurement much and therefore the uncertainty remains largely unchanged. When $$K\_t$$ is close to $$1$$, we are very confident in our measurement and therefore will have low uncertainty.
## Putting it all together
Finally, we have each of the equations we need for our single input-single output Kalman filter. Now we can put them together and then we will be able to effectively implement our filter in software. Taking each simplified part and putting them into a computable procedure will yield:
**Set Initial Conditions**
$$x=\text{initial system state}$$
$$q=\text{model variance}$$
$$r=\text{sensor variance}$$
$$P=\text{initial guess}$$
$$K=\text{initial guess}$$
**loop:**
1. $$x\_t=x\_{t-1}+u\_t$$ project our state estimate
2. $$p\_t = p\_{t-1} + q$$ project our variance
3. $$K\_t = \frac{p\_t}{p\_t+r}$$ calculate the Kalman gain
4. $$x\_t = x\_{t-1}+K\_t(z\_t-x\_t)$$ update estimation based on sensor reference
5. $$p\_t=(1-K\_t)p\_t$$ update error variance
## Kalman Filter Pseudocode
```java
double x = 0; // your initial state
double Q = 0.1; // your model covariance
double R = 0.4; // your sensor covariance
double p = 1; // your initial covariance guess
double K = 1; // your initial Kalman gain guess
double x_previous = x;
double p_previous = p;
double u = 0;
double z = 0;
while (true) {
u = getInput(); // Ex: change in position from odometry.
x = x_previous + u;
p = p_previous + Q;
K = p/(p + R);
z = getSecondSensor(); // Pose Estimate from April Tag / Distance Sensor
x = x + K * (z - x);
p = (1 - K) * p;
x_previous = x;
p_previous = p;
}
```
Finally, you have now implemented one of the most important filters in modern control theory.
### Implementation Example
Here you can find a Jupyter notebook for a Kalman filter that fuses a motion profile and velocity sensor:
## Multiple Sensors
You can also use multiple sensors with the Kalman Filter! For each loop, you can first run a prediction and then for each additional sensor can have an update loop. In some cases, however, you'd be better off using a weighted average of all the sensor readings.
## Limitations of our work
For simplicity's sake, we made the assumption that the majority of systems we will be dealing with in FTC are single-input, single-output systems. Unfortunately, this is not guaranteed and you may have to end up with a more complicated filter where you must use matrices instead of scalar values. For that, you will need to use a library such as [EJML ](http://ejml.org/wiki/index.php?title=Main_Page)and remove lots of the simplifications that we made.
# Gain Scheduling
Some systems may fail with a simple linear controller such as PID. Let's fix that!
### Why Gain Scheduling?
Controllers like PID and Full State Feedback are inherently linear. They compose of simple operations such as multiplication, subtraction, integration, and derivative calculation. This means that the controllers are easy to design and understand. Unfortunately, we live in the real world. In the real world, there is almost **never** such a thing as a linear system. We are generally able to assume that our system is linear **enough**, but sometimes, this simply isn't true. This is where gain scheduling comes into play.
### What is Gain Scheduling?
Gain scheduling is the idea where we modify our controller parameters relative to where our system is in space. Imagine a rotating arm on your robot. You begin by tuning your controller to stabilize in an upright position such as this:
You are happy with the performance of having the arm swing up to the vertical position, so you proceed to test it in a horizontal position:
You find that after requesting your arm to move to 0°, it ends up settling below your target angle. Initially, you are confused, as it worked perfectly for the 90° setpoint. But then, you remembered that your system is not linear! As a result of your system being nonlinear, the linear controller you tuned for one setpoint may not cover the dynamics of the entire **operating region**.
{% hint style="info" %}
**Operating Region**: the space in which the system operates. If you only operate your system in a small region, it is more likely that a linear controller will be adequate.
{% endhint %}
A solution to this problem and the inherent concept of gain scheduling is that we can change our controller coefficients depending on where our system is in the operating region.
Let's say that this is our current code:
```java
// Imaginary PID controller and coefficient objects
PIDCoefficients for90Degrees = new PIDCoefficients();
PIDController controller = new PID(for90Degrees);
while (loopIsRunning) {
double armAngle = getArmAngle();
double targetAngle = getReference();
double output = controller.calculate(targetAngle, armAngle);
arm.setPower(output);
}
```
The code sample above has its PID coefficients tuned to operate at the 90° range. As we saw with the previous example, it fails whenever we go to 0°.
One approach we can take is to split our operating region into two halves — suppose, in the middle, at 45°. Then, we will use one set of coefficients above 45° and one set below.
```java
// imaginary PID controller and coefficients objects
PIDCoefficients above45Degrees = new PIDCoefficients(); // tune for above
PIDCoefficients below45Degrees = new PIDCoefficients(); // tune for below
PIDController controller = new PID(for90Dgrees);
while (loopIsRunning) {
double armAngle = getArmAngle();
double targetAngle = getReference();
if (armAngle < 45) {
controller.setCoefficients(below45Degrees);
} else {
controller.setCoefficients(above45Degrees);
}
double output = controller.calculate(targetAngle, armAngle);
arm.setPower(output);
}
```
This approach works well but doesn't necessarily scale the best. If you had 10 different operating regions, that means you would have to create an if/else statement for each one. That would get messy very quickly. In addition, setting your target angle to 45° could have a bit of extra oscillation as it switches gains every time it passes over the switching point.
An approach to improve this is to use interpolation.
A really simple way to accomplish this is to use the FTCLib InterpLUT [(Interpolated Look Up Table)](https://docs.ftclib.org/ftclib/v/v1.2.0/features/util#interplut-interpolated-look-up-table).
An interpolated look-up table will take a key (the current angle) and then give you an estimate of what our PID coefficients should be at the given point. It does this by allowing us to put in a few known values, and then it will generate a continuous function to go between them.
```java
// look up tables for PID coefficients
InterpLUT pCoefficients = new InterpLUT();
InterpLUT iCoefficients = new InterpLUT();
InterpLUT dCoefficients = new InterpLUT();
pCoefficients.add(0, KpAt0);
pCoefficients.add(90, KpAt90);
iCoefficients.add(0, KiAt0);
iCoefficients.add(90, KiAt90);
dCoefficients.add(0, KdAt0);
dCoefficients.add(90, KdAt90);
pCoefficients.createLUT();
iCoefficients.createLUT();
dCoefficients.createLUT();
while (loopIsRunning) {
double armAngle = getArmAngle();
double targetAngle = getReference();
double Kp = pCoefficients.get(armAngle);
double Ki = iCoefficients.get(armAngle);
double Kd = dCoefficients.get(armAngle);
PIDCoefficients coefficients = new PIDCoefficients(Kp, Ki, Kd);
controller.setCoefficients(coefficients);
double output = controller.calculate(targetAngle, armAngle);
arm.setPower(output);
}
```
{% hint style="success" %}
You will now have a very robust controller powered by gain scheduling!
{% endhint %}
{% hint style="info" %}
You aren't limited to just PID. You can also use this method to adjust your Feedforward or use Full State Feedback instead of PID.
{% endhint %}
# Motion Profiling
What are motion profiles, and how can they help us in FTC?
## 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 FTC robots, applying maximum power from a standstill position will cause *slip*, which is undesirable because it results in rough movement, traction loss, and disrupted wheel odometry.
### 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 use 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 profiles
The most common type of motion profile in FTC 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 limiting 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 in FTC.
#### How do we implement trapezoidal motion profiling?
There's a few different ways to implement trapezoidal motion profiling in code, but something that all of the ways will have in common is the use of kinematic formulas. The kinematic formulas are `velocity = starting velocity + acceleration * time` and `position = starting position + starting velocity * time + (1/2) * acceleration * time`.
Here's an example of a pseudocode motion profile implementation.
```java
double motion_profile(max_acceleration, max_velocity, distance, elapsed_time) {
"""
Return the current reference position based on the given motion profile times, maximum acceleration, velocity, and current time.
"""
// Calculate the time it takes to accelerate to max velocity
acceleration_dt = max_velocity / max_acceleration
// If we can't accelerate to max velocity in the given distance, we'll accelerate as much as possible
halfway_distance = distance / 2
acceleration_distance = 0.5 * max_acceleration * acceleration_dt ** 2
if (acceleration_distance > halfway_distance) {
acceleration_dt = Math.sqrt(halfway_distance / (0.5 * max_acceleration))
}
acceleration_distance = 0.5 * max_acceleration * acceleration_dt ** 2
// recalculate max velocity based on the time we have to accelerate and decelerate
max_velocity = max_acceleration * acceleration_dt
// we decelerate at the same rate as we accelerate
deceleration_dt = acceleration_dt
// calculate the time that we're at max velocity
cruise_distance = distance - 2 * acceleration_distance
cruise_dt = cruise_distance / max_velocity
deceleration_time = acceleration_dt + cruise_dt
// check if we're still in the motion profile
entire_dt = acceleration_dt + cruise_dt + deceleration_dt
if (elapsed_time > entire_dt) {
return distance
}
// if we're accelerating
if (elapsed_time < acceleration_dt) {
// use the kinematic equation for acceleration
return 0.5 * max_acceleration * elapsed_time ** 2
}
// if we're cruising
else if (elapsed_time < deceleration_time) {
acceleration_distance = 0.5 * max_acceleration * acceleration_dt ** 2
cruise_current_dt = elapsed_time - acceleration_dt
// use the kinematic equation for constant velocity
return acceleration_distance + max_velocity * cruise_current_dt
}
// if we're decelerating
else {
acceleration_distance = 0.5 * max_acceleration * acceleration_dt ** 2
cruise_distance = max_velocity * cruise_dt
deceleration_time = elapsed_time - deceleration_time
// use the kinematic equations to calculate the instantaneous desired position
return acceleration_distance + cruise_distance + max_velocity * deceleration_time - 0.5 * max_acceleration * deceleration_time ** 2
}
}
```
### Motion Profile Usage
To use a motion profile, you must use some controller, such as a PID or Proportional Controller.
```java
while (TrajectoryIsNotDone) {
double instantTargetPosition = motion_profile_position(max_acceleration, max_velocity, distance, elapsed_time);
double motorPower = (instantTargetPosition - motor.getPosition()) * Kp
}
```
You can also extend this further by using velocity/acceleration feedforward.
```java
while (TrajectoryIsNotDone) {
double x = motion_profile_position(max_acceleration, max_velocity, distance, elapsed_time);
double v = motion_profile_velo(max_acceleration, max_velocity, distance, elapsed_time);
double a = motion_profile_accel(max_acceleration, max_velocity, distance, elapsed_time);
double motorPower = (x - motor.getPosition()) * Kp + Kv * v + Ka * a;
}
```
You now know how to implement one of the most powerful control strategies in FTC!
# SMARTDAMP Algorithm
Our custom approach to PID tuning
More content soon. TLDR: if you have the kV and kA feedforward values. we can use that as a model for our system.
Then, we solve for the poles of the system. After some math, we arrive at the following:
$$
K\_D=2\sqrt{K\_aK\_P}-K\_v
$$
This will guarantee a critically damped PID response for a PD controller, given an arbitrary choice of Kp
It is recommended that the following inequality is true:
$$
K\_p \geq \frac{K\_v^2}{4K\_a}
$$
Otherwise, Kd will be negative and you get a scary non-minimum phase system.
### Specifying percent overshoot
A Critically damped system is not necessarily the fastest possible response, sometimes you are okay with a bit of overshoot. This modified version of the SMARTDAMP algorithm allows you to specify a percent overshoot in addition to your proportional gain to give you your optimal choice for Kd
$$
K\_d = 2 \zeta \sqrt{K\_a K\_p} - K\_v
$$
where Zeta is our [dampening ratio](https://en.wikipedia.org/wiki/Damping#Damping_ratio_definition).
We can find zeta using the following equation given *PO* (Percent overshoot)
$$
\zeta =\frac{-\ln{\frac{PO}{100}}}{\sqrt{\pi^2 + \ln^2{\frac{PO}{100}}}}
$$
# Extra Resources
Extra resources to continue learning
*FIRST* Tech Challenge Discord:
Book for learning code:
Code Examples:
Roadrunner library for FTC:
FTC specific software guides:
Controls Engineering in the *FIRST* Robotics Competition (Graduate-level control theory for high schoolers):
Stability of the Kalman Filter for Output Error Systems: [https://hal.inria.fr/hal-01232177/document#:\~:text=The%20stability%20of%20the%20Kalman,process%20noise%20is%20totally%20absent.](https://hal.inria.fr/hal-01232177/document)
or email us at:
# Video Tutorials
A series of tutorials that will aid in your mastery of FTC Control Theory.
{% embed url="" %}
Intro to advanced Controls topics such as Full state feedback, Feedforward, and the Kalman Filter
{% endembed %}
{% embed url="" %}
PID Controller Implementation Tutorial #1
{% endembed %}
{% embed url="" %}
PID Tutorial #2 Angle Control
{% endembed %}
# Glossary
Let's be real, the nomen clature around control theory is incredibly intimidating at first; it should't be like this. Also this is not in alphabetical order lol.
1. Reference - The target position we would like to be at. This is often used interchangably with other terms such as set point or target.
2. Gain - A value we multiply by another value. In a PID controller Kp, Ki, and Kd are "gains"
3. Feedback - a process where given a measurement and a desired state we calculate the best input to reach the state. A synonym for closed-loop control
4. Feedforward - a process where given just a desired state we calculate the best input to reach the state. A synonym for open-loop control
5. PID control - A type of feedback control with three main components. A proportional, an integral, and a derivative.
6. Integral - the sum of a signal over time
7. Derivative - the rate of change of a singal
8. Full State Feedback - a feedback control method that is basically just a bunch of proportional controllers strapped together.
9. State - where our system currently is, usually a combination of position / velocity.
10. RoadRunner - A library for *FIRST* Tech Challenge that incorperates a ton of really powerful control techniques. It has built in localization (using drive encoders or dead wheels), trajectory generation, and trajectory following algorithms
11. Path - a set of points that makes up where our robot should be in space. Usually traversed through by the robots current position.
12. Trajectory - a set of position and velocity points, usually indexed by the given time.
13. State machine - a way to code your robots that emulates asynchronous behavior but without the side effects.
# FTC Motor Control
This page will help teams that may be new to FTC begin by starting with basic motor movement and then working them through the various different ways to control such a motor.
{% hint style="info" %}
The Goal of this section is to provide an alternative to the built in RUN TO POSITION mode in the FTC Control System. We advise teams to use their own, external PID rather than the in built PID because it runs at a faster refresh rate and thus will have much greater stability than the inbuilt controller. In addition, an external controller allows the flexbility to use one or more custom feedforward controls to further improve the performance of your system.
{% endhint %}
## Setting up your development environment.
Before we start we are going to assume you have an editor such as Android Studio setup to modify the SDK and that your motors hardware map is completed. If not, please check out this [guide](https://gm0.org/en/latest/docs/software/using-android-studio.html) before proceeding. We will also assume for now that you have a basic opmode setup.
### Setting up your motor
For this example, we are going to assume that we are controlling a simple motor such as one that may be on an arm or on your robot's drivetrain. This motor is assumed to have a motor encoder plugged into the Rev expansion hub. The motor encoder port is generally on the back of the motor and will require a cable that is provided by the motor's manufacturer. The purpose of your motor's encoder is that measures the position of the motor so that we can use control techniques to move it accurately to the desired position.
#### Here you can see how we can create a new motor in our hardware map
```java
public class tutorial extends LinearOpMode {
// motor declaration, we use the
// Ex version as it has velocity measurements
DcMotorEx motor;
@Override
public void runOpMode() throws InterruptedException {
// the string is the hardware map name
motor = hardwareMap.get(DcMotorEx.class, "arm");
// use braking to slow the motor down faster
motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// disables the default velocity control
// this does NOT disable the encoder from counting,
// but lets us simply send raw motor power.
motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
waitForStart();
}
}
```
Above there are a few things happening, for starters, on line 3 we are creating an object that is of type DcMotorEx, this is an extended version of the traditional DcMotor class that adds functionality such as measuring the velocity of the motor. On line **8**, once the user presses init we initialize the motor to the given name that you defined earlier in the hardware map. If you haven't done this yet you should probably stop and get this out of the way now. Next on line **12,** we are setting the motor to use braking, this allows our motor to decelerate much faster than the alternative float mode.
On line **14** we are doing something that looks a little bit off but it is what we want. **RUN WITHOUT** ENCODERS does **NOT** disable the encoder from doing things such as position or velocity measurement but instead lets us just assign raw motor power to our motor. The alternative mode, RUN USING ENCODERS simply runs the motor at a speed proportional to its maximum speed, for example setting the power at 0.5 would run the motor at exactly 50% of its speed. While this may sound great at first, this internal velocity control runs at a slower frequency and will often have less desirable results than using our own custom, external controller. For this external controller to work properly we should be using RUN WITHOUT ENCODER.
{% hint style="danger" %}
If you glanced over that previous paragraph **don't**. Read it again, I promise it's important.
{% endhint %}
#### Getting the motor moving.
Now that we have a motor configured properly we can now start making it move!
```java
public class tutorial extends LinearOpMode {
// motor declaration, we use the
// Ex version as it has velocity measurements
DcMotorEx motor;
@Override
public void runOpMode() throws InterruptedException {
// the string is the hardware map name
motor = hardwareMap.get(DcMotorEx.class, "arm");
// use braking to slow the motor down faster
motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// disables the default velocity control
// this does NOT disable the encoder from counting,
// but lets us simply send raw motor power.
motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
waitForStart();
// loop that runs while the program should run.
while (opModeIsActive()) {
motor.setPower(0.5);
}
}
}
```
In the code snippet above, we added a while loop that runs for the duration of the program and each time the loop runs we are assigning a value at 50% of the motors maximum current.
{% hint style="info" %}
If we just wanted to assign a single motor power and be done with it, the above loop would be redundant as assigning motor powers to persist until you assign it again using setPower.
{% endhint %}
#### Closing the Loop on our motor
In FTC 99% of the time \[citation needed but probably true] we want to drive our motor to the desired position. A wonderful way to accomplish this in the context of FTC is the PID Controller. We checked out this controller in a [previous chapter](/the-pid-controller) and we recommend checking this out to gain an intuition of the controller's structure before continuing with this chapter.
When making a PID Controller it is likely a good idea to abstract this into it's own class for easy use. Here is the basic structure we recommend to start with that you can fill out on your own using the lessons learned from previous chapters:
```java
public class PIDController {
/**
* construct PID controller
* @param Kp Proportional coefficient
* @param Ki Integral coefficient
* @param Kd Derivative coefficient
*/
public PIDController(double Kp, double Ki, double Kd) {
}
/**
* update the PID controller output
* @param target where we would like to be, also called the reference
* @param state where we currently are, I.E. motor position
* @return the command to our motor, I.E. motor power
*/
public double update(double target, double state) {
// PID logic and then return the output
}
}
```
Once we have our PID controller created we can then use it in our opmode similarly to this:
```java
public class tutorial extends LinearOpMode {
// motor declaration, we use the
// Ex version as it has velocity measurements
DcMotorEx motor;
// create our PID controller, you will need to tune these parameters
PIDController control = new PIDController(0.05,0,0);
@Override
public void runOpMode() throws InterruptedException {
// the string is the hardware map name
motor = hardwareMap.get(DcMotorEx.class, "arm");
// use braking to slow the motor down faster
motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// disables the default velocity control
// this does NOT disable the encoder from counting,
// but lets us simply send raw motor power.
motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
waitForStart();
// loop that runs while the program should run.
// position in encoder ticks where we would like the motor to run
int targetPosition = 100;
while (opModeIsActive()) {
// update pid controller
double command = control.update(targetPosition,
motor.getCurrentPosition());
// assign motor the PID output
motor.setPower(command);
}
}
}
```
{% hint style="success" %}
You now have an opmode that, using your PID controller will drive to the desired position!
{% endhint %}
{% hint style="info" %}
There are a few places we can go to now to improve this
1. you can implement one or more [feedforward](/feedforward-control) controllers to enhance the system's stability
2. You can use .getVelocity() instead of getting position and turn this into a velocity controller if your heart so desires. (This is why we use DcMotorEx).
3. You can implement a [finite state machine](https://gm0.org/en/latest/docs/software/finite-state-machines.html) paradigm so that your PID controller will continue to update
4. Every time you get the [urge to use threads](https://gm0.org/en/latest/docs/software/control-system-internals.html) ... simply don't and see step three.
1. The reason for this according to Game Manual 0 is that "LynxCommands being blocking (and more specifically a per-bus master lock being present) means that multithreading hardware calls are at best not helpful and typically harmful to performance" ([Control System Internals](https://gm0.org/en/latest/docs/software/control-system-internals.html)).
{% endhint %}
####
# Drivetrain Control
Drivetrains are arguably the most important part of a \*FIRST\* Tech Challenge autonomous and are crucial to get correct.
## Why is Drivetrain Control Hard?
In FTC we often want to drive our robot to a desired x, y, and angle position on the field. This requires two equally important things:
* A way to measure your robots position
* A way to move your robot to a desired position
In recent years, the idea of odometry has become increasingly popular in *FIRST* Tech Challenge. Odometry simply means that we are using sensors to observe the position of your robots drive train. The most common method of odometry in FTC is using encoders either on the drive wheels of your robot or with seperate sprung wheels known as [dead wheels. ](https://gm0.org/en/latest/docs/robot-design/dead-wheels.html)There exists several popular libraries for doing the position estimation math but the two that we recommend are the three wheel localizer in the [Roadrunner quick start](https://github.com/acmerobotics/road-runner-quickstart) and [HolonomicOdometry](https://github.com/FTCLib/FTCLib/blob/master/core/src/main/java/com/arcrobotics/ftclib/kinematics/HolonomicOdometry.java) in FTCLib.
Once you have a method of robot localization picked out we can begin deciding how we want to control our robot.
Traditionally in FTC, teams used their drive encoders to drive straight some distance, switch to using the gyro to turn, and repeat for the duration of their drivetrain control. While this method is simple and effective, it is not necessarily the most robust solution. Modern FTC control systems rely on a concept known as pose stabilization. Pose stabilization uses a single controller to move each axis of your system (x,y, theta) to zero error.
## Mecanum Drivetrain Controller
To start with controlling a mecanum drivetrain we first need for figure out how to control each robot relative axis. A quick look at the [game manual 0 mecanum programming](https://gm0.org/en/latest/docs/software/mecanum-drive.html) guide details that given an x,y, and theta control input we can control a mecanum robot like the following:
```java
// x, y, theta input mixing
frontLeftMotor.setPower(x + y + t);
backLeftMotor.setPower(x - y + t);
frontRightMotor.setPower(x - y - t);
backRightMotor.setPower(x + y - t);
```
The code above will allow us to very easily move our robot in the coordinate space relative to itself.
In order to move our robot we need to [rotate](https://en.wikipedia.org/wiki/Rotation_matrix) the x and y commands by the robots angle:
To perform this rotation we can take our x command, y command, and the angle of our robot and use the rotation equation to solve for the rotated commands
```java
double x_rotated = x * Math.cos(angle) - y * Math.sin(angle);
double y_rotated = x * Math.sin(angle) + y * Math.cos(angle);
```
{% hint style="success" %}
FTClib users can accomplish this built in by using Vector2d.rotateby(angle) method.
{% endhint %}
we can then combine this rotation with our mecanum control mixing to get the following:
```java
// rotation
double x_rotated = x * Math.cos(angle) - y * Math.sin(angle);
double y_rotated = x * Math.sin(angle) + y * Math.cos(angle);
// x, y, theta input mixing
frontLeftMotor.setPower(x_rotated + y_rotated + t);
backLeftMotor.setPower(x_rotated - y_rotated + t);
frontRightMotor.setPower(x_rotated - y_rotated - t);
backRightMotor.setPower(x_rotated + y_rotated - t);
```
{% hint style="success" %}
If one was to use gamepad joystick inputs for x,y,t they can use this code above to accomplish field relative driving!
{% endhint %}
We are at the point now that given any input, x,y,t it will push our robot in the direction regardless of the angle, we can even combine multiple inputs such as moving along diagonal paths while rotating. We have reached a state where our system is now fully actuated and nearly completely linearized. This means from this point on our system is now relatively trivial to control.
For getting the values of x,y, and t we can simply use a linear controller such as the full state feedback or PID controllers that we developed in previous chapters
```java
xControl = new PID();
yControl = new PID();
thetaControl = new PID();
while (loopIsActive) {
// this imaginary pid controller has a control method that uses the
// PID controller we defined earlier in a method called calculate
// the first argument of calculate is the reference state.
// the second is the systems state.
x = xControl.calculate(xTarget, xRobotPosition);
y = yControl.calculate(yTarget, yRobotPosition);
t = thetaControl.calculate(thetaTarget, thetaRobotPosition);
double x_rotated = x * Math.cos(angle) - y * Math.sin(angle);
double y_rotated = x * Math.sin(angle) + y * Math.cos(angle);
// x, y, theta input mixing
frontLeftMotor.setPower(x_rotated + y_rotated + t);
backLeftMotor.setPower(x_rotated - y_rotated + t);
frontRightMotor.setPower(x_rotated - y_rotated - t);
backRightMotor.setPower(x_rotated + y_rotated - t);
}
```
{% hint style="danger" %}
For thetaControl you will need to consult the [Dealing with Angles](https://www.ctrlaltftc.com/controlling-heading) chapter for changing the way that the angle error calculation is performed. This will ensure your robot is always turning the shortest distance to the desired angle
{% endhint %}
{% hint style="success" %}
After completing all of these steps you will now have a fully functional pose stabilization controller for a mecanum robot!
{% endhint %}
{% embed url="" %}
Disturbance rejection qualities of similar control technique
{% endembed %}
## Differential Drivetrain Controller
Differential drive robots such as 4 wheel drive, 6 wheel drive, and tank tread robots are unfortunately a beast to control. This is because of one unfortunate quality that differential drive robots possess: They are underactuated systems.
In control theory, an underactuated system is defined as a system that has fewer actuators than they do outputs to control. Intuitively it makes sense as a fully actuated system such as a mecanum drive can move towards the desired x,y position while turning towards the desired angle. A differential drive robot cannot do this. Instead it needs to first turn towards the target x,y position, move towards the target position and then once at the x,y position turn to the desired angle.
### Naive Differential Drive Controller
While not perfect, this approach to differential drive control can be very effective. This controller works by first driving to the desired position, and then once the robot is within a desired distance to the position turns toward the target angle.
This controller requires feedback on three seperate values
* The distance to the target point
* The angle to the target point
* The reference angle
{% hint style="success" %}
Remember: The angle to the target point is the angle between the robot and the target x,y position. The reference angle is the angle we want the robot to be facing whenever it finishes the movement.
{% endhint %}
Using the following will drive the robot at a power proportional to the distance between the robot position and the target position
```java
distanceController = new PID();
while (loopIsActive) {
// 0 is the reference because we want the distance to go to 0
double f = distanceController.calculate(0,getDistance(robotX,robotY,tagetX,targetY));
double t = 0;
frontLeftMotor.setPower(f + t);
backLeftMotor.setPower(f + t);
frontRightMotor.setPower(f - t);
backRightMotor.setPower(f - t);
}
```
Now we have code that given the robots position and a target position will drive proportional to its remaining distance. We still need to make the robot turn towards the target point otherwise it will continue on in whatever direction the robot started in forever.
For this, we first need to find a way to calculate the angle between the robot and the target. Fortunately for us, there is a built in java method to accomplish this known as atan2. Atan2 is a special trigomentric function that given arguments y,x returns the angle from x,y to the origin. Since we are getting the angle between two points we use the error between the robot x,y and the target x,y as follows:
```java
double xError = targetX - robotX;
double yError = targetY - robotY;
double theta = Math.atan2(yError,xError);
```
{% hint style="warning" %}
Make sure that you use your y value as the first argument followed by the x value as the java [Math library javadoc](https://docs.oracle.com/javase/8/docs/api/java/lang/Math.html) states is required for the atan2 method to function correctly.
{% endhint %}
now that we can calculate this angle we can put this into the control loop for our turn command:
```java
distanceController = new PID();
angleController = new PID(); // make sure this follows "Dealing with Angles"
while (loopIsActive) {
double xError = targetX - robotX;
double yError = targetY - robotY;
double theta = Math.atan2(yError,xError);
// 0 is the reference because we want the distance to go to 0
double distance = Math.hypot(xError, yError);
double f = distanceController.calculate(0,distance);
double t = angleController.calculate(theta, robotTheta);
double left_power = f + t;
double right_power = f - t;
}
```
{% hint style="danger" %}
I understand you are probably tired of hearing this but for angles and feedback control you MUST FOLLOW THE [DEALING WITH ANGLES CHAPTER](https://www.ctrlaltftc.com/controlling-heading) OR YOUR CONTROLLER ***WILL*** BREAK.
{% endhint %}
Now we have a controller that will drive to a desired x,y position on the field but unfortunately will not stop at the desired angle. A quick but unfortunately not very elegant solution is to simply check if we are within an acceptable distance and simply switch to turning the robot:
```java
distanceController = new PID();
angleController = new PID(); // make sure this follows "Dealing with Angles"
while (loopIsActive) {
double xError = targetX - robotX;
double yError = targetY - robotY;
double theta = Math.atan2(yError,xError);
// 0 is the reference because we want the distance to go to 0
double distance = Math.hypot(xError, yError);
double left_power = f + t;
double right_power = f - t;
if (distance < threshold) {
f = 0;
t = angleController.calculate(targetAngle, robotTheta);
}
// set motor power
}
```
{% hint style="success" %}
Now you have a controller that will drive to a desired x,y, theta position
{% endhint %}
#### Cosine Trick
In order to get better stability, we can slow the drive trains forward movement while the angle error is high.
Doing something like the following
$$
p\_f=d*k\_p*\cos{\theta\_e}
$$
where the power in the forward direction is equal to Kp times the distance as we had before but then an addition cos term is added. \*Theta\_error will be domain limited to -pi/2 to pi/2\*
Looking at the graph of cos shows an interesting curve, whenever our angle error is high, the value of this cos approaches zero, but as the angle error approaches zero, the cos term goes to 1. This creates the effect of only allowing our robot to quickly move in the forward direction when it is on target.
```java
distanceController = new PID();
angleController = new PID(); // make sure this follows "Dealing with Angles"
while (loopIsActive) {
double xError = targetX - robotX;
double yError = targetY - robotY;
double theta = Math.atan2(yError,xError);
// 0 is the reference because we want the distance to go to 0
double distance = Math.hypot(xError, yError);
double left_power = f + t;
double right_power = f - t;
if (distance < threshold) {
f = 0;
t = angleController.calculate(targetAngle, robotTheta);
} else {
f = distanceController.calculate(0, distance);
t = angleController.calculate(theta, robotTheta);
}
// Range.clip is included in the SDK and will clip between two values
// angleController.error is a demonstrative attribute that gets the error.
f *= Math.cos(Range.clip(angleController.error, -PI/2, PI/2));
// set motor power here!
setRobotRelative(f,t);
}
```
# Dealing with Angles
A very common use case for PID controllers in FTC is turning your robot to a desired heading. However, a few modifications are necessary.
Many may find that attempting to use your PID controller to turn your robot to a desired angle will cause your robot to turn the longest way possible or spin unnecessary rotations.
This issue arises from the simple nature that angles do not go on forever and instead wrap around. Imagine our calculation for error that we did in previous chapters: `error = reference - state`
`reference` is where we would like to be, and the `state` is where we currently are. This method is fantastic for measurements that continue forever, such as that of an encoder, but fails to perform properly for wrapping measurements.
For example, let's assume that our robot is facing an angle of 1 degree. We would like to turn our robot to face 359 degrees. We can calculate the error between these two as `359 - 1 = 358`. This result is very suboptimal; out of the two possible ways the robot could have turned, it chose the longest possible way, turning nearly a full rotation the other way. Accounting for angle wrap can solve this with just a little bit of code:
```java
// This function normalizes the angle so it returns a value between -180° and 180° instead of 0° to 360°.
public double angleWrap(double radians) {
while (radians > Math.PI) {
radians -= 2 * Math.PI;
}
while (radians < -Math.PI) {
radians += 2 * Math.PI;
}
// keep in mind that the result is in radians
return radians;
}
```
Example borrowed from: [FTC Programming: Pure Pursuit Tutorial 1 by FTC 11115 Gluten Free](https://www.youtube.com/watch?v=3l7ZNJ21wMo)
We can then calculate our error as the following:
```java
Math.toDegrees(angleWrap(Math.toRadians(359 - 1)));
```
which then performs the following operations:
> 359 - 1 = 358
>
> 358 > 180
>
> 358 - 360 = -2
>
> new corrected angle = -2
Using this angle wrap method will ensure that your PID controller turns in the desired direction and will prevent numerous issues such as your robot potentially spinning infinitely in circles. There are more efficient ways to implement this, such as using the modulus operator, but that is something up to you to do for yourself. See [Road Runner's angle utility](https://github.com/acmerobotics/road-runner/blob/96a59ae121806780fd2d4086f388309a7870caa4/core/src/main/kotlin/com/acmerobotics/roadrunner/util/Angle.kt) for one such example.
{% hint style="success" %}
The FTC SDK provides the [AngleUnit#normalize(radians)](https://github.com/OpenFTC/Extracted-RC/blob/4f328d0b0f14dffeba68356a4c0170b9b2074619/RobotCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/AngleUnit.java#L197) function that performs exactly the same operation as the our `angleWrap` above.
{% endhint %}
# Roadrunner Control Explained
Roadrunner is a library used by many successful teams, and understanding why we utilize it in specific ways and not others is key to avoiding common pitfalls.
The Roadrunner library and Quickstart project accomplish three main things:\
1\. Localization - using sensors such as drive encoders or [dead wheels](https://gm0.org/en/latest/docs/common-mechanisms/dead-wheels.html) to estimate your robot's position on the field.\
2\. Trajectory Generation - Roadrunner lets you create paths your robot follows. These paths are unique because they also calculate your robot's exact position, velocity, and acceleration at each point in time. Trajectories make your autonomous routines more consistent.\
3\. Control - Using algorithms like [PID](/the-pid-controller) and [feedforward](/feedforward-control), which have previously been covered on CTRL ALT FTC, we can achieve high performance in following roadrunner's trajectories.
### Why velocity PID is not recommended
On the FTC discord and other community forums, it is often recommended to use feedforward instead of velocity PID, even when using drive encoders!
{% hint style="success" %}
Individuals new to the FTC may believe the recommendation to set RUN\_ *USING \_ ENCODERS (RUE) to FALSE* will disable the use of encoders entirely. Instead, all it does is disable the use of velocity PID for the .setPower method of the motors. With RUE enabled, the motors will use PID to attempt to maintain a velocity as a percentage of the maximum.
{% endhint %}
1. Feedforward has less phase lag than PID Control.\
\
For PID control to produce a motor command, there must be an error between the current and desired states. This means that, by definition, your system is not perfectly following the velocity/acceleration trajectory. Feedforward models your robot's dynamics and produces nearly perfect (ish) motor powers to reach the desired trajectory state.
2. Feedforward is more stable than PID Control.\
Since velocity control is asymptotically stable, meaning that if you give your motor an arbitrary power, it will converge to a constant velocity, there isn't a need to have the precise feedback of a PID loop. Sensor noise from how velocity is calculated from the encoders can also result in further degraded performance.
#### But what about robustness?
A concern arising from the suggestion not to use feedforward is that your robot will not be able to reject disturbances as a PID Controller would. What is missing from this, though, is that you still will use PID! Since you also need to tune a position control loop, you aren't losing ANY robustness to disturbances. You are only making it easier to respond to these changes by using feedforward.
Visual Overview of how the roadrunner control system is constructed
As we can see from the graphic above, regardless of whether we use feedforward or PID, we will still get corrections from the position PID, ensuring robustness.
# What is Homeostasis?
Introduction to the Homestasis library by FTC #19376 Thermal Equilibrium
Homeostasis is a library in development by FTC #19376 Thermal Equilibrium. Homeostasis aims to provide an easy way to rapidly implement advanced control techniques with as slight a headache as possible. The end goal for Homeostasis is to provide a one size fits all solution to control and state estimation in *FIRST* Tech Challenge.
We identified that, for the most part, one could very elegantly abstract most code for FTC control systems away, and with Homeostasis, we aim to provide a controls library that plays nicely with the majority of programming paradigms used in FTC, such as that of the [state machine](https://gm0.org/en/latest/docs/software/finite-state-machines.html) and [command-based](https://docs.ftclib.org/ftclib/command-base/command-system) paradigms.
Currently homestasis provides the following features:
* Implementations of many controllers
* PID Control
* Full State Feedback
* Bang Bang Control
* Basic, Gravity, and Arm Feedforward
* Estimation algorithms to improve sensor readings:
* Low Pass Filter
* Least Squares + Kalman Filter
* Many useful utilities such as the WPILib Motion Profile and functions to deal with angles.
* Systems to easily utilize the aforementioned algorithms in a unified way.
* BasicSystem
* A combination of an Estimator, Feedback, and Feedforward Controller
* PositionVelocitySystem
* A combination of (2) Estimators (one for position, one for velocity), (2) feedback controllers (one for position, one for velocity) and (1) Feedforward.
{% hint style="danger" %}
Currently homeostasis is in Beta and while in many cases it's algorithms have been confirmed to work very well, in some edge cases they could still potentially fail. Please let us know or create a github issue / PR if you encounter any issues at:
{% endhint %}
# Installation
In this chapter we will detail how to install the homeostasis library
### Gradle
Navigate to `build.dependencies.gradle` and find `repositories`.
In repositories put the maven link to jitpack.io
```groovy
repositories {
... // other repositories that are already there
maven { url 'https://jitpack.io' }
}
```
Then add the dependency:
```groovy
implementation 'com.github.Thermal-Equilibrium:homeostasis-FTC:1.0.8'
```
Now resync your gradle and Homeostasis should be installed!
# Included Controllers
Homeostasis provides a few different types of controllers, all of which with their own utility.
## PID Control
### BasicPID
```java
// creation of the PID object
PIDCoefficients coefficients = new PIDCoefficients(Kp,Ki,Kd);
BasicPID controller = new BasicPID(coefficients);
// usage of the PID
while (true) {
double output = controller.calculate(targetPosition, measuredPosition);
}
```
BasicPID.java is a primitive PID implementation that is good to get something working quickly but may not be optimal for all situations. Many issues such as derivative noise and integral windup may cause issues with the controller. As a result we have created "PIDEx"
### PIDEx
```java
// creation of the PID object
PIDCoefficientsEx coefficients = new PIDCoefficientsEx(Kp,Ki,Kd,integralSumMax
stability_thresh,
lowPassGain);
// usage of the PID
PIDEx controller = new PIDEx(coefficients);
while (true) {
double output = controller.calculate(targetPosition, measuredPosition);
}
```
This PID controller implements a few crucial features on top of the traditional PID Controller.
1. Integral reset on zero crossover: As the error crosses over zero, we reset the integral to prevent moving in the wrong direction.
2. We have a stability threshold argument, basically if the derivative (effectively speed) is above this threshold we deem there is no reason to run the integral calculation. This helps remove overshoot.
3. We cap the integral sum of our PID controller to ensure that we don't saturate our system. This helps with integral windup. For traditional FTC motors where the power is -1 < x < 1, integralSumMax \* Ki should be less than or equal to 1.
4. We use a low pass filter on the derivative. This helps remove noise from the measurements.
## Full State Feedback Control
Full State feedback is an approach where we perform simultaneous feedback on each state (position, velocity, etc) of our system in parallel. To make this easy we use a custom vector class that is effectively an extension of an array that provides helpful methods such as the dot product.
### FullStateFeedback
```java
// This example is a two state, position velocity system.
// Similar to the PID coefficients
Vector K = new Vector(new double[] {1.1,0.3});
FullStateFeedback controller = new FullStateFeedback(K);
while (true) {
// measured position and velocity
Vector state = new Vector(new double[]{position, velocity});
// target position and velocity
// protip - these can be generated with motion profiles!
Vector reference = new Vector(new double [] {referencePosition, referenceVelocity});
double command = controller.calculate(reference,state);
}
```
This type of controller works especially well with motion profiles. Homeostasis uses a trapezoidal motion profile borrowed from WPILib. We also recommend, if available to use the roadrunner motion profiles. This controller is able to follow references along multiple states at once, allowing it to perform better in these type of trajectories than PID.
## Bang Bang Control
While not recommended for the majority of tasks, Bang Bang control can often be the right tool for the job if one needs a very aggressive controller and doesn't mind either oscillations or large steady state error.
### BangBang
```java
double maxOutput = 1;
double tolerance = 3;
BangBangParameters parameters = new BangBangParameters(maxOutput,tolerance);
BangBang controller = new BangBang(parameters);
while (true) {
double command = controller.calculate(referencePosition, position);
}
```
This controller takes in two parameters, it's output power and a tolerance. This controller works by making a simple comparison: is it too low or is it too high and then applies either maxOutput or -maxOutput to the system. If the error is within tolerance (or hysteresis as the parameter is called), then the controller will return 0. This hysteresis prevents the bang bang controller from oscillating forever but has the trade off of introducing some state error.
## Dealing with Angles
The previously mentioned linear feedback controllers will fail in certain circumstances such as using them in combination with an IMU to turn the robot. For this case it is recommended to use the AngleController wrapper to correct this. This wrapper implements the techniques discussed in the "[Dealing with Angles](https://www.ctrlaltftc.com/controlling-heading)" section of CTRL ALT FTC.
Here we can see how to use this wrapper:
```java
double targetAngle = Math.toRadians(90);
PIDCoefficients coefficients = new PIDCoefficients(0.5,0,0);
// any controller that implements "FeedbackController" can be used.
BasicPID pid = new BasicPID(coefficients);
AngleController controller = new AngleController(pid);
while (true) {
double command = controller.calculate(targetAngle, readIMURadians());
}
```
An important note is that AngleController can be used with nearly controller in homeostasis and even your own! Just make sure that this controller implements the "FeedbackController" interface!
## Feedforward Control
As we have learned from the rest of the website, Feedforward control is an open loop controller, meaning it only takes in the target state and does not require a measurement. Generally this works very well with velocity control in FTC due to the tendency for velocity measured by encoders to be quite noisy; making closed loop control difficult.
### BasicFeedforward
This implements a basic, Velocity, Acceleration, Static controller
```java
double Kv = 1.1;
double Ka = 0.2;
double Ks = 0.001;
FeedforwardCoefficients coefficients = new FeedforwardCoefficients(Kv,Ka,Ks);
BasicFeedforward controller = new BasicFeedforward(coefficients);
while (true) {
double command = controller.calculate(0,referenceV,referenceA);
}
```
This controller takes in three arguments, a target position, a target velocity, and a target acceleration. Please note, **the target position will not actually do anything in BasicFeedforward**. This controller will then use Kv and Ka to approximate the desired motor output. Ks stands for static and is the minimum power in either direcion that our motor can output. This is to be tuned in a way that allows our system to respond much more quickly at lower reference velocities.
### FeedforwardEx
This controller takes BasicFeedforward and adds better support for arm and elevator systems.
```java
double Kv = 1.1;
double Ka = 0.2;
double Ks = 0.001;
// Kg and Kcos should NOT be used together, one or the other should be zero.
double Kg = 0.1;
double Kcos = 0.1;
FeedforwardCoefficientsEx coefficientsEx = new FeedforwardCoefficientsEx(Kv,Ka,Ks,
Kg,Kcos);
FeedforwardEx controller = new FeedforwardEx(coefficientsEx);
while (true) {
double command = controller.calculate(x,v,a);
}
```
This is a more advanced feedforward controller that extends the basic feedforward controller. It adds angle compensation for arms and gravity compensation for elevators / linear slides. The intent is that Kg and Kcos should be used exclusively and not with one another. Kg provides a constant upward torque to counteract gravity and Kcos is multiplied by the reference angle of your lift to compensate for the nonlinear effects of it rotating. **IF Kcos is USED X MUST BE IN RADIANS.** With Kcos and Kg, these should be tuned so the system can hold its weight without feedback.
## No Control
When using a system where we don't want a particular type of control (we only want a feedforward or a feedback controller, not both) there exists NoFeedback and NoFeedforward which simply act as dummy classes.
# State Estimation and Filters
The utility of sensors in FTC can be improved with a variety of algorithms.
{% hint style="success" %}
While technically incorrect nomenclature, each of the individual algorithms themselves will be implemented as a Filter, then they will also have to be implemented as an estimator to be used with Systems.
{% endhint %}
## Filters
### Low pass filter
The low pass filter is a basic algorithm that allows one to smooth values given to it, this is used previously in CTRL ALT FTC to help compensate for derivative noise and is used by PIDEx for this purpose.
```java
double a = 0.9; // low pass gain, must be within 0 < x < 1
LowPassFilter filter = new LowPassFilter(a);
while (true) {
double currentValue = readNoisySensor(); // imaginary, noisy sensor
double estimate = filter.estimate(currentValue); // smoothed sensor
}
```
### Least Squares + Kalman Filter
Using the Kalman Filter discussed previously on CTRL ALT FTC and Linear Least Squares regression as the model, we have developed a very robust way to remove noise from signals.
```java
double Q = 0.3; // High values put more emphasis on the sensor.
double R = 3; // High Values put more emphasis on regression.
int N = 3; // The number of estimates in the past we perform regression on.
KalmanFilter filter = new KalmanFilter(Q,R,N);
while (true) {
double currentValue = readNoisySensor(); // imaginary, noisy sensor
double estimate = filter.estimate(currentValue); // smoothed sensor
}
```
This Kalman Filter implementation has three parameters to tune. First **Q** is the sensor covariance, or how much we trust the sensor, low values for the sensor means that we believe the sensor will have lots of noise and vice versa. **R** is the model covariance or how much we trust the linear regression. **N** is the number of elements back we perform the regression on. we find that for most cases between 3 and 5 works best.
### Kalman Filter vs Low Pass Filter
If both the Kalman FIlter and the Low Pass filter reduce noise, and the Low Pass filter is so much simpler than the Kalman Filter, why should the Kalman Filter exist?
The answer: Phase Lag
The low pass filter removes high frequency signals from your measurements. While this does remove lots of noise from the signal (as noise is generally high frequency), it also slows down the signals response. The Kalman Filter does not have nearly as much phase lag since it works by projecting it's state forward instead of focusing soley on previous measurements.
{% hint style="info" %}
For each of these filters, there also exists an "Estimator", each estimator is basically just a wrapper around each of these filters that uses a double supplier for more convenient use in 'systems'.
{% endhint %}
### Estimators
```java
double Q = 0.3;
double R = 3;
int N = 3;
DoubleSupplier sensor = new DoubleSupplier() {
@Override
public double getAsDouble() {
return readNoisySensor();
}
};
Estimator estimator = new KalmanEstimator(sensor,Q,R,N);
while (true) {
double estimate = estimator.update(); // look! no arguments!
}
```
Estimators with their usage of DoubleSuppliers allow the passing of sensor values into the filter to be much more seemless.
# Systems in Homeostasis
Throughout the world, systems containing many parts work together to achieve homeostasis; Our robot is no different.
### BasicSystem
Basic systems include, a single Estimator, a single Feedback controller, and a single Feedforward Controller.
```java
BasicSystem(Estimator estimator, FeedbackController feedbackController,
FeedforwardController feedforwardController);
```
{% hint style="success" %}
If you wish to not use any of these features that is okay! Just pass in the "No" or "Raw" versions of each interface!
{% endhint %}
### PositionVelocitySystem
This is a slightly more complex system that attempts to control both position and velocity at the same time. This uses two feedback controllers, two estimators, and a single feedforward.
```
public PositionVelocitySystem(Estimator positionEstimator,
Estimator velocityEstimator,
FeedforwardController feedforward,
FeedbackController positionFeedback,
FeedbackController velocityFeedback);
```
### Example systems
#### Basic Motor Control
```java
PIDCoefficients coefficients = new PIDCoefficients(0.3,0.04,0.01);
DoubleSupplier motorPosition = new DoubleSupplier() {
@Override
public double getAsDouble() {
return exampleMotor.getPosition();
}
};
BasicPID controller = new BasicPID(coefficients);
NoFeedforward feedforward = new NoFeedforward();
RawValue noFilter = new RawValue(motorPosition);
BasicSystem system = new BasicSystem(noFilter,controller,feedforward);
while (true) {
double command = system.update(referencePosition);
}
```
The above example demonstrates the basic syntax for systems.
#### Full State with PID
Effectively Full State feedback and Full State Estimation using two PID's and two Kalman Filters.
```java
double Q = 0.3;
double R = 3;
int N = 3;
PIDCoefficients posCoefficients = new PIDCoefficients(0.1,0,0);
PIDCoefficients veloCoefficients = new PIDCoefficients(0.1,0,0);
BasicPID posControl = new BasicPID(posCoefficients);
BasicPID veloControl = new BasicPID(veloCoefficients);
DoubleSupplier motorPosition = new DoubleSupplier() {
@Override
public double getAsDouble() {
return exampleMotor.getPosition();
}
};
DoubleSupplier motorVelocity = new DoubleSupplier() {
@Override
public double getAsDouble() {
return exampleMotor.getVelocity();
}
};
KalmanEstimator positionFilter = new KalmanEstimator(motorPosition,Q,R,N);
KalmanEstimator velocityFilter = new KalmanEstimator(motorVelocity,Q,R,N);
FeedforwardCoefficients coefficientsFF = new FeedforwardCoefficients(0.1,0.3,0.001);
BasicFeedforward feedforward = new BasicFeedforward(coefficientsFF);
PositionVelocitySystem system =
new PositionVelocitySystem(positionFilter,
velocityFilter,feedforward,posControl,veloControl);
while (true) {
// x,v,a is short hand for position, velocity, acceleration targets
double command = system.update(x,v,a);
}
```
#### Feedforward Only
Sometimes we might not have a sensor available to us and as a result feedforward is our only option!
```java
double Kv = 0.1;
double Ka = 0.3;
double Ks = 0.001;
double Kg = 0.1;
double Kcos = 0;
Estimator none = new NoSensor();
NoFeedback none2 = new NoFeedback();
FeedforwardCoefficientsEx coefficientsEx = new FeedforwardCoefficientsEx(Kv,Ka,Ks,Kg,Kcos);
FeedforwardEx controller = new FeedforwardEx(coefficientsEx);
BasicSystem system = new BasicSystem(none,none2,controller);
while (true) {
double command = system.update(x,v,a);
}
```