How does PID control work on an FTC arm?

PID control on an FTC arm often involves compensating for the effect of gravity, which changes depending on the arm's angle . The relative force of gravity on an arm can be calculated as the cosine of the angle the arm makes with the horizontal, where zero degrees is straight to the side . This calculated gravity effect can then be used as a feedforward parameter for a PID controller .

A common approach for gravity-compensated feedforward involves adding a kF constant multiplied by the cosine of the arm's current angle to the PID output . The kF constant, which approximates the power needed to counteract gravity, is typically found empirically by setting PID gains to zero and adjusting kF until the arm can hold its position at any angle .

The angle of the arm can be determined using an encoder . The DEGREE_PER_TICK value, necessary for this calculation, is found by dividing 360 by the encoder's resolution and multiplying by the gear ratio . The pseudocode for calculating the current angle is current_angle = (TICKS_AT_ZERO - current_tick) * DEGREE_PER_TICK .

For controlling a single motor's velocity or position, the built-in PID controller can be used by setting the run mode to RUN_USING_ENCODER . However, the built-in PID controller has a fixed refresh rate of 20Hz (50ms sample rate) . For higher performance and more stable control, some teams use an external PID controller, which allows for higher sample rates, potentially up to 80Hz . When using an external PID controller, it is generally recommended to use RUN_WITHOUT_ENCODER mode .

While the proportional (Kₚ) and derivative (Kd) terms are often sufficient, the integral term (Kᵢ) can be used to correct for situations where the motor isn't supplied enough power to reach the target, by summing the error over time and increasing the output . To prevent lag from affecting the sum, the error is multiplied by the time taken for the previous loop iteration before being added to the integral term .

Was this right?

Sources6

Control Loops > PID > Built-In PID Controller

Game Manual 0·CC BY-NC 4.0

For situations where one needs to control the velocity or position of a single motor, the built in PID controller can be used. PID can be enabled by changing the run mode to RUN_USING_ENCODER

Hint: Many misunderstand the use of RUN_USING_ENCODER, many may mistake that it is necessary to use this mode for the encoders to work at all, but this is not true. Instead, RUN_USING_ENCODER enables velocity feedback using the encoder. If you are using an external PID controller such as one that you implement, generally, it is recommended that you use RUN_WITHOUT_ENCODER.

For official documentation on the built in PID controller, see here (https://docs.revrobotics.com/duo-control/programming/using-encoder-feedback)

Open Game Manual 0

Control Systems

Game Manual 0·CC BY-NC 4.0

The FTC® control system is based around a "Robot Controller" and a "Driver Station". The Robot Controller is mounted on the robot. It is either embedded within or connected to special "Hub(s)", which in turn connect to motors, servos, and sensors. The Robot Controller is linked to the Driver Station through WiFi or WiFi Direct.

REV Robotics is the sole manufacturer of legal FTC control system components. The REV Expansion Hub connects to motors, servos, sensors, and a Robot Controller. A REV Control Hub has the same functionality of an Expansion Hub but with a built-in Robot Controller.

More information about the FTC Control System can be found below:

Official Control System Documentation on FTC Docs (https://ftc-docs.firstinspires.org/en/latest/programming_resources/shared/control_system_intro/The-FTC-Control-System.html)
REV Control System Documentation (https://docs.revrobotics.com/duo-control/)
Official troubleshooting guide (https://ftc-resources.firstinspires.org/ftc/archive/2026/team/robot-troubleshooting)

There are two possible control systems that can be run on an FTC robot legally:

REV Control Hub + REV Expansion Hub
RC Phone + REV Expansion Hub(s)

Open Game Manual 0

Control Loops > PID > PID Controller Sample Rate

Game Manual 0·CC BY-NC 4.0

For teams who desire the most performance out of their PID controller, it is essential to consider the Sample rate of the controller. The Sample rate is when the controller updates its output given new sensor data. Higher Sample rates allow for more stable control and allow for the usage of more significant PID coefficients to reduce settling time. See this video (https://www.youtube.com/watch?v=fusr9eTceEo&t=133s) to see how sample rate effects stability in a practical motor control example. The inbuilt PID controller is locked at a 20hz refresh rate (50ms sample rate). Many top FTC teams optimize their robot loops to run at up to 80hz, achieving much more stable control with an external PID.

Open Game Manual 0

Control Loops > Feedforward Control > Gravity Compensated Feedforward

Game Manual 0·CC BY-NC 4.0

In gravity-compensation we derive the effect of gravity upon an arm as Fg = gcos{theta}. Here we can use that with the following logic.

while True:
    error = desire_position - current_position; # no effect on gravity compensation
    current_angle = (TICKS_AT_ZERO - current_tick) * DEGREE_PER_TICK
    output = PID(error) + cos(radians(current_angle)) * kF

This code uses a kF constant to approximate how much power the arm needs to counteract gravity. In theory, this is something related to gravity multiplied by the rotational moment of inertia of your arm. Calculating this is impractical and kF is instead found empirically. This can be done by setting the gains on the PID to 0, and increasing kF until the arm can hold itself up at any position. If your arm is still falling, increase kF, and if your arm is moving upwards, decrease kF. FTC team 16379 Kookybotz has an excellent video on Arm programming (https://youtu.be/E6H6Nqe6qJo?si=AztzVtAqShFEA4EP&t=440), where they demonstrate how to increase kF.

Open Game Manual 0

Kinematics > Manipulators

Game Manual 0·CC BY-NC 4.0

Often in FTC, teams have arms that swing out around an axis. Controlling these arms requires a bit of thought as depending on the angle they're at, the effect of gravity drastically changes.

Our first step is defining a reference frame. Our recommendation is to define zero as straight to the side, as this is when gravity has the greatest effect on your arm. Other reference frames will still work, but the exact trigonometry will change.

(Figure: 11329 I.C.E. Robotics)

In a reference frame like this, the relative force of gravity will be equal to the cos of the angle. This makes sense as when the cos function is evaluated at zero degrees, it returns 1, while at 90 degrees where there is no effect of gravity, it returns 0.

Fg = gcosθ

Assuming you have an encoder on your arm, you can find theta (the angle of the arm relative to the vertical) using something similar to the following pseudocode.

Note: DEGREE_PER_TICK can be found by taking 360 divided by your encoder resolution all multiplied by your gear ratio. For example, for a 19.2:1 goBILDA Yellow Jacket (https://www.gobilda.com/5203-series-yellow-jacket-planetary-gear-motor-19-2-1-ratio-24mm-length-8mm-rex-shaft-312-rpm-3-3-5v-encoder/), there are 537.7 ticks per revolution, and so frac{360}{537.7} degrees per tick at the gearbox output shaft. If there was a 2:1 gear ratio after that, then the ticks per revolution would instead be frac{360}{2 times 537.7}

current_angle = (TICKS_AT_ZERO - current_tick) * DEGREE_PER_TICK

The typical way to utilize the effect of gravity you just found, is to pass it in as a feedforward parameter to a PID controller. We cover this in gravity-compensated-feedforward.

Open Game Manual 0

Control Loops > PID > The Integral Term

Game Manual 0·CC BY-NC 4.0

Admittedly, the integral term is the least important term for FTC® PID control loops. With a properly tuned Kₚ and Kd, you often can just set Kᵢ to 0 and call it a day.

However, it can still be useful in some cases. Just like the derivative term, the integral term intends to correct for overshoot. if the system thinks it reached its destination, it will stop, even when, in fact, the error is not yet 0. Perhaps the motor is no longer being supplied enough power to move. Well, given enough time, the integral term will increase the output (in this case, motor power), causing movement towards the destination.

To explain without calculus, the integral term essentially sums the error over a specific interval of time. To do this, error in each loop iteration is added to a variable (in this case, I(t)).

However, summing error this way has an unfortunate side effect: the longer the loop takes to complete one iteration, the more slowly this sum increases, which is obviously not desirable, as we don't want lag to affect how the robot moves. To compensate for this, before the error is added to I(t), it is multiplied by how long the previous loop took to-complete, or t_n+1}-tn, preventing lag from making the system sum more slowly.

So say the robot stops short of the target. The P and D combination aren't strong enough to move it forward to the destination. You can either tune Kₚ and Kd to compensate (this is recommended), or you can add the integral term to increase output (this works too, but requires more attention and tuning to achieve the same result).

Open Game Manual 0