Skip to content
MARSLib Documentation

Control Theory Mastery

Basic PID loops are reacting to the past. To achieve World Championship fidelity, your mechanisms must predict the future. MARSLib employs advanced control theory algorithms, including Feedforwards and State Space Models.

1. Feedforward: Predicting the Physics

A PID loop only acts when there is an error. A Feedforward (FF) model anticipates the physical energy required to reach a state. For a heavy elevator (like the Ladder climber), gravity is always pulling it down. A ElevatorFeedforward calculates the exact voltage needed to counteract gravity (kG) before the PID even kicks in.

// kS (Static Friction), kG (Gravity), kV (Velocity), kA (Acceleration)
ElevatorFeedforward ff = new ElevatorFeedforward(0.1, 0.4, 1.2, 0.05);


// Calculate voltage required to hold steady, PLUS voltage to reach target velocity
double ffVoltage = ff.calculate(currentVelocity, targetVelocity);
double pidVoltage = pid.calculate(currentPosition, targetPosition);


motor.setVoltage(ffVoltage + pidVoltage);

2. Manual Tuning Guide

If you cannot run SysId, you must manually tune your Feedforwards and PID loops. Always tune Feedforward (FF) first, as PID should only be correcting minor disturbances, not doing the heavy lifting.

  1. Set everything to Zero: kP = 0, kI = 0, kD = 0, kS = 0, kV = 0, kG = 0, kA = 0.
  2. Tune kS (Static Friction): Slowly increase kS until the mechanism just barely starts to twitch or overcome friction.
  3. Tune kG (Gravity): For vertical elevators, increase kG until the elevator holds its current position without falling. For rotary arms, position the arm perfectly horizontal (where gravity is strongest) and tune kG until it holds steady.
  4. Tune kV (Velocity): Command the mechanism to run at a consistent voltage (e.g. 6v). Log the resulting steady-state velocity (e.g. 5 rad/s). Your kV = 6.0 / 5.0 = 1.2.
  5. Tune kP (Proportional): Now that the FF model handles the physics, increase kP to snap the mechanism quickly to its target. Stop increasing when it starts oscillating around the setpoint.
  6. Tune kD (Derivative): If you have slight overshoot or oscillation, increase kD to act as a dampener to slow the system down as it approaches the target.

3. Flywheel Inertia Recovery Simulator

Flywheels do not fight gravity, but they require massive energy to accelerate (Inertia) and maintain high speeds (Friction/Air Drag). When a game piece enters the shooter, it rapidly saps energy from the wheel. kV keeps the wheel spinning, while kP and kD are crucial for Recovery Time.

How to manually tune a Flywheel:

  1. Set kP = 0, kI = 0, kD = 0. Adjust the kV slider until the Actual velocity (red) perfectly tracks the Setpoint velocity (blue) in steady state (e.g. at 80 rad/s). In this simulator, that happens around kV = 0.12.
  2. Once the wheel holds its setpoint on its own, click INJECT BALL to introduce a massive physical drag disturbance.
  3. Notice the horrific recovery time using only kV? Increase kP to aggressively spike voltage during the disturbance to violently snap the wheel back to the setpoint!
  4. Use kD (Derivative) if your kP is causing the wheel to overshoot past 80 rad/s. A small kD acts like a parachute, slowing down the acceleration as the error rate rapidly shrinks.
  5. Use kI (Integral) sparingly if the velocity gets “stuck” right underneath the setpoint (e.g. 78 rad/s) because kP isn’t strong enough. In systems with high friction, the integral mathematically “builds up” over time, forcing that last 2 rad/s error to zero.

4. Rotating Arm (Cosine Feedforward)

Unlike an Elevator where gravity pulls equally at all times, a rotating Arm or Intake Pivot experiences gravity differently depending on its angle. When perfectly horizontal (0°), gravity exerts maximum torque. When perfectly vertical (90° or -90°), the center of mass aligns with the fulcrum, meaning gravity exerts zero torque.

An ArmFeedforward recalculates gravity every 20ms using kG * Math.cos(angle). Try moving the arm to 90° below to see the required kG voltage drop to zero!

How to manually tune an Arm:

  1. Change the Target Angle to exactly 0° (Horizontal). This is where gravity pulls the hardest.
  2. Set kP = 0, kI = 0, kD = 0 to disable PID. Adjust kG until the FF Voltage precisely counters the physical drop and holds the arm perfectly level.
  3. With FF holding the arm up perfectly against gravity, increase kP to help the arm quickly snap to new angles.
  4. If the arm oscillates (wobbles back and forth) before settling, add a small amount of kD (Derivative). It will act like a shock absorber, damping the motion as it approaches the target.
  5. If the arm gets stuck incredibly close to its target (e.g. 88° when asking for 90°), use a small kI (Integral) to wind up enough voltage to clear the final friction barrier.
  6. Move the Target Angle to 90° (straight up). Watch the kG voltage organically shrink to zero and the arm stabilize effortlessly without PID fighting!

Looking for SysId?

MARSLib recommends mathematically automating all kS, kV, and kA calculations. If you want to skip manual tuning and let WPILib calculate perfection, check out our dedicated SysId Characterization Tutorial!



📖 Further Reading & External Resources