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).

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

FIRST Tech Challenge Discord: https://discord.gg/first-tech-challenge

Book for learning code: https://github.com/alan412/LearnJavaForFTC

Code Examples: https://github.com/Thermal-Equilibrium/FTC-Control-Theory-Examples

Roadrunner library for FTC: https://www.learnroadrunner.com/

FTC specific software guides: https://gm0.org/en/stable/docs/software/index.html

Controls Engineering in the FIRST Robotics Competition (Graduate-level control theory for high schoolers): https://file.tavsys.net/control/controls-engineering-in-frc.pdf

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.

or email us at: [email protected]

Control System Example

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 and make a pull request.

Thanks to everyone who has contributed!

Made with .

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

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.

Gradle

Navigate to build.dependencies.gradle and find repositories.

In repositories put the maven link to jitpack.io

repositories {
    ... // other repositories that are already there
    maven { url 'https://jitpack.io' }

}

Then add the dependency:

implementation 'com.github.Thermal-Equilibrium:homeostasis-FTC:1.0.8'

Now resync your gradle and Homeostasis should be installed!

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 chapter.

Here we can see the response from this system:

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

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:

Example borrowed from:

We can then calculate our error as the following:

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 for one such example.

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:

Another example of open loop control is that of an intake which runs continuously:

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

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.

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!

The Roadrunner library and Quickstart project accomplish three main things: 1. Localization - using sensors such as drive encoders or dead wheels 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 and feedforward, 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!

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.

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.

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

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:

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:

Otherwise, Kd will be negative and you get a scary non-minimum phase system.

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 and paradigms.

Currently homestasis provides the following features:

• Implementations of many controllers

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.

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.

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.

Estimators

Estimators with their usage of DoubleSuppliers allow the passing of sensor values into the filter to be much more seemless.

If you recall from our and the from earlier, we can roughly control a systems state using the formula:

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:

​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.

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.

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.

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.

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?

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.

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.

BasicSystem

Basic systems include, a single Estimator, a single Feedback controller, and a single Feedforward Controller.

PID Control

BasicPID

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

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.

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

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

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 "" section of CTRL ALT FTC.

Here we can see how to use this wrapper:

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

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.

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.

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:

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.

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: 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:

In this equation, 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 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 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:

Here, represents the variance of and 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, , 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:

In this equation, is the value of our measurement and Kt is a constant between and that we will later define. The expression 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 , 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 , then and if , . The closer is to , the more trust we put in the measurement, and the closer it is to , the more trust we put in our model.

Let’s now compute the Kalman gain as follows:

is the variance in our measurement. You may notice that this formula is a simple proportion, and that makes a lot of sense. If is high compared to , will have a value close to . This makes sense because if there is more uncertainty in our model than our measurement we should weigh the measurement more, which a value close to achieves. In the opposite case, when is high compared to then has a value close to , favoring the model more than the measurement.

Alas, when we update our estimated value we must also update its uncertainty.

Since ranges from to , will also range from and , so our measurement will always decrease uncertainty in our estimate. When is close to , we do not factor in the measurement much and therefore the uncertainty remains largely unchanged. When is close to , 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

loop:

1. project our state estimate

2. project our variance

3. calculate the Kalman gain

4. update estimation based on sensor reference

Kalman Filter Pseudocode

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 and remove lots of the simplifications that we made.

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.

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 There exists several popular libraries for doing the position estimation math but the two that we recommend are the three wheel localizer in the and 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 guide details that given an x,y, and theta control input we can control a mecanum robot like the following:

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 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

we can then combine this rotation with our mecanum control mixing to get the following:

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

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

Using the following will drive the robot at a power proportional to the distance between the robot position and the target position

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:

now that we can calculate this angle we can put this into the control loop for our turn command:

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:

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

​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.