# Intro to Control Theory

Control algorithms are tools used to control the motion and state of robot components. For example, controlling the position/angle of an arm, controlling the speed of the robot, or targeting a turret. Different control algorithms are used in different scenarios, and can often be layered together for better control. The common control algorithm used in FRC is the PID (proportional-integral-derivative) controller, however there are several others commonly used, either alone or together with a PID controller.

Control algorithms can be broadly categorized into open-loop (also called feed-forward) and closed-loop (feedback) controllers. Open-loop control doesn't use any information from sensors (sensor feedback), and the controller itself will behave the same way regardless of the behavior (position, speed, temperature, current draw, etc.) of the actual robot component. Closed-loop controllers use information from at least one sensor, allowing the controller to dynamically adjust based on the behavior of the robot.

# Timer

The simplest example of an open-loop controller is a timer - run a motor (or another actuator, such as a pneumatic piston, relay, etc.) for a certain amount of time and hope whatever the motor is powering goes far enough but not too far. Timer control is almost never used - friction, another robot, low battery, etc. could all cause the motor to not move far enough.

An example use for a timer might be to turn off power (using a relay) to a specific electronic component for a specific amount of time, in order to force it to restart.

# Bang-bang

Bang-bang controllers are an extremely simple way to use a sensor in order to reach a target - spin a shooter up to a target speed, move the robot a specific distance, etc. They are the simplest example of a closed-loop controller, and are primarily useful for very simple control that doesn't need to be fast or precise. A bang-bang controller drives a motor (or other actuator) at a constant voltage/output until the sensor passes the target value, at which point the controller stops powering the motor entirely, or even runs it backwards until the sensor passes the target value in the other direction, and the controller goes back to the first direction.

An example of a good use for a bang-bang controller would be spinning a ball shooter up to 2,500 RPM. The bang-bang controller could run the shooter motors at 100% power until the speed passes 2,500 RPM, and then turn off the motor until friction slows it down below 2,500 RPM, at which point the controller would power the motors at 100% power again, and so on. If the shooter has enough inertia and the motor controllers are placed into coast mode rather than brake mode, the shooter speed will hover near 2,500 RPM and appears to be smooth. If the shooter doesn't have enough inertia (e.g. the flywheel is too light), the shooter wheel speed might oscillate back and forth between too slow and too high every time the bang-bang controller changes from 0% to 100% motor output.

Another example would be trying to drive the robot forward a specific distance - for example, most years the FRC game has an autonomous task to simply cross a line. This could be a place to use a very simple bang-bang controller. Using encoder sensors on the robot's wheels makes it possible to calculate how far the robot has driven (distance = wheel revolutions x wheel circumference). A bang-bang controller could drive the robot forward at say 50% power until the wheels have spun enough times to have driven more than 10 feet, then drive the robot backward a little bit so it doesn't go too far and get in the way of other robots. Since a robot will stop quickly when driving at low speeds, rather than running forever like a shooter RPM controller, a distance bang-bang controller can simply turn off once the sensor says the robot is close enough to the target distance. This will prevent the robot from oscillating back and forth at the target distance forever.

Bang-bang controllers generally lack the precision necessary to control the position of an arm, the angle of a swerve module, the height of an elevator, high-speed driving control, etc. However for simpler tasks like spinning on a shooter wheel, driving forward slowly (or turning slowly), using a vacuum pump to draw a specific strength vacuum, bang-bang controllers are great since they take very little work to setup and will often perform just as well (or in some cases, better) than more complicated control algorithms.

# Take-Back-Half

A take-back-half controller is a closed-loop controller useful for controller the velocity of something, usually a shooter wheel. It works extremely well for shooters where a simple bang-bang controller might cause too much oscillation. The idea is that the error (difference between current sensor value, and the target sensor value) is integrated to calculate the motor output, until the velocity (e.g. shooter RPM) crosses the target value, at which point the output is set to halfway between the current output and a special value called the take-back-half variable, and the take-back-half variable is set to that output. When the velocity slows down and drops back below target, the output is again set to halfway between the current output and the new take-back-half variable. If this sounds complicated, don't worry too much, the actual math is quite simple. The effect of using a take-back-half controller is rapid convergence to the target value, at which point the velocity will stabilize.

Now for the actual math. Let s be the target velocity, p be the current velocity, d be the current motor output, e be the error, g be the gain (a tuning parameter used to adjust the performance), and b be the take-back-half variable. Then each time the controller is updated, e = s - p, and d = d + (g x e). If the error was positive last update and is now negative, or vice versa, then d = b = 0.5(d + b).

To tune the take-back-half controller for optimum performance, you will need to estimate a motor output value (call it m)that will, given enough time, cause the shooter (or other mechanism) to spin at close to the desired speed. It doesn't need to be super precise, just close enough to get the controller in the right ballpark quickly. Then set d to 0.5 (you could set it to 1 and immediately run the shooter at full speed, however this can produce too much torque on the shooter mountings and potentially damage the robot - if the shooter speeds up too quickly lower this value, if it speeds up too slowly you could try setting it higher). Set the initial value for the b (the take-back-half variable) to (2xm)-1. Then set the gain to a small number, such as 1e-5. Finally, test it output and adjust the gain until it behaves as desired. As with any flywheel shooter, it is a good idea to put the motor controllers into coast mode to prevent them from slowing down too quickly if the robot is disabled.

# PID

PID controllers are incredibly versatile and powerful, and so are used all over the place on FRC robots. PID controllers are in many ways a bang-bang controller on many, many, many steroids. They are closed-loop controllers that incorporated feedback from a sensor and use it to slow down as the target state is reached (for an arm, this would mean as the angle for the arm gets closer to the target angle the controller will slow down the arm so it doesn't overshoot), compensate for friction or other things slowing down progress towards the goal (for example if a robot was trying to go 15 feet forward and ran into another robot, the PID controller would being increase motor output until the robot began moving again), and compensate for momentum and oscillation.

## The Math Behind the PID

The basic idea behind a PID controller is to start by calculating the error (the difference between the value the sensor is reading, and the value we want the sensor to be reading) at a specific time. At time t the error is given by e(t). Plugging it into the following equation gives the output control signal that should be sent to the motor, pump, heater, etc. Kp, Ki, and Kd are constants used to adjust how the controller behaves for different applications - different motor powers, different friction, different weight, etc. Tuning these constants for a particular system can be time consuming and is usually done primarily through trial and error, but when done correctly yields a PID capable of quickly and precisely moving to a target without overshooting.

The output of the PID function is the output control signal that should be sent to the motor, pump, heater, etc. The function is comprised of three terms: the proportional, the integral, and the derivative.

The proportional term outputs a value that is proportional to the error. If the error is large (far away from target), the proportional term is large. If the error is small, the proportional term is small. This means the PID controller outputs a large enough value to make whatever it is controlling (called the process) approach the target value fast enough, but as it gets closer to the target it slows down until it reaches no output when it is right at the target. This helps prevent overshoot while allowing it to reach the target in a reasonable amount of time, however there are a few issues. Often the output from the proportional becomes too small to overcome friction (or other forms of resistance) when it is close to the target and the error is relatively small. This means whatever is being controller will never quite reach the target, and always have a small error, known as the steady state error.

The integral terms main purpose is to help overcome steady state error. Understanding the math behind the integral term needs some first-year calculus, however it can be understand intuitively. The integral term tracks whether the error has been consistently above or below zero, and adds some additional output that gets bigger and bigger the longer the error is not zero. This creates a few issues when a PID controller needs to make a big change - even when the proportional term is doing its job just fine, the integral term will buildup overtime. When the process is finally approaching the target, the proportional term will shrink down to zero, but the integral term won't start shrinking until the process has overshot and the error has become negative. There are several techniques use to overcome this problem, which is known as integral windup. These involve placing limits on how large the integral term can become, and motion profiling or other scheduling, which is discussed below.

The derivative term generally serves to counter-act momentum or other effects that similarly make things keep moving even when the output control signal is zero. The derivative term subtracts from the others (or adds when they are negative), and gets larger (absolute value gets larger that is) the faster the error is changing. When the error is large, the proportional term overpowers the derivative term, but as the error and the proportional term approach zero, the derivative dominates and serves to slow the process down before it overshoots.