Motion Control (PID)
This page focuses on use of PID for controlling position of subsystems rather than velocity of subsystems(Haven’t had a shooting game yet) or other use cases(See AutoAlignMotionPlanner.java)
Overview
In our 2025 robot code, all PID control logic for the motor controllers is hidden within the ServoMotorSubsystem class. This design abstracts away vendor-specific details and allows easy configuration and tuning through subsystem constants.
Instead of manually implementing PID loops, the ServoMotorSubsystem handles closed-loop control internally, using parameters defined in the subsystem constants. This promotes clean, maintainable, and consistent motor control across the robot.
What is PID Control?
Proportional-Integral-Derivative (PID) control is a method of precisely controlling motors by continuously adjusting motor outputs based on the error between a target (setpoint) and the current measured position or velocity.
It operates in a closed loop feedback system: the controller takes sensor input, compares it to the desired setpoint, and computes an output to minimize error over time.
How PID Works (Simplified)
P (Proportional): Produces an output proportional to the current error. The larger the error, the stronger the correction.
I (Integral): Accounts for the accumulation of past errors to correct steady-state offset.
D (Derivative): Reacts to the rate of change of the error, acting as a damping factor to reduce overshoot.
The combination of these components allows smooth, stable control of motor position or velocity despite disturbances or varying loads.
Example: Intake Deploy Subsystem Constants
/**
* Constants related to the Intake Deploy subsystem.
*/
public static final class DeployConstants {
public static final ServoMotorSubsystemConstants kDeployServoConstants = new ServoMotorSubsystemConstants();
public static final AbsoluteEncoderConstants kDeployEncoderConstants = new AbsoluteEncoderConstants();
static {
kDeployServoConstants.kName = "Deploy";
kDeployServoConstants.kMainConstants.id = Ports.INTAKE_PIVOT;
kDeployServoConstants.kMainConstants.counterClockwisePositive = false;
kDeployServoConstants.kHomePosition = 0; // degrees
kDeployServoConstants.kRotationsPerUnitDistance = (1.0 / 360.0) * 20;
kDeployServoConstants.kMaxUnitsLimit = 0;
kDeployServoConstants.kMinUnitsLimit = -90;
// PID Gains
kDeployServoConstants.kKp = 3.8125;
kDeployServoConstants.kKi = 0.0;
kDeployServoConstants.kKd = 0;
kDeployServoConstants.kKa = 0;
kDeployServoConstants.kKs = 0;
kDeployServoConstants.kKv = 0.5;
kDeployServoConstants.kKg = 0.265625;
// Motion Profiling
kDeployServoConstants.kCruiseVelocity = 32 * 360 * 1 / 20;
kDeployServoConstants.kAcceleration = 32 * 360 * 1 / 20;
// Output Limits
kDeployServoConstants.kMaxForwardOutput = 12.0;
kDeployServoConstants.kMaxReverseOutput = -12.0;
// Current Limits
kDeployServoConstants.kEnableSupplyCurrentLimit = true;
kDeployServoConstants.kSupplyCurrentLimit = 80; // amps
kDeployServoConstants.kSupplyCurrentThreshold = 80; // amps
kDeployServoConstants.kSupplyCurrentTimeout = 0.01; // seconds
kDeployServoConstants.kEnableStatorCurrentLimit = true;
kDeployServoConstants.kStatorCurrentLimit = 80; // amps
kDeployServoConstants.kNeutralMode = NeutralModeValue.Brake;
// Encoder settings
kDeployEncoderConstants.rotor_to_sensor_ratio = 20;
kDeployEncoderConstants.remote_encoder_port = Ports.INTAKE_CANCODER;
// Homing parameters
kDeployServoConstants.kHomingOutput = -0.3;
kDeployServoConstants.kHomingTimeout = 0.2;
kDeployServoConstants.kHomingVelocityWindow = 5;
}
}Parameters
kKp, kKi, kKd
Proportional, Integral, Derivative PID gains that control motor response to error.
kKa, kKs, kKv, kKg
Feedforward gains accounting for acceleration, static friction, velocity, and gravity effects.
kCruiseVelocity, kAcceleration
Motion profiling parameters controlling max velocity and acceleration during moves.
kMaxForwardOutput, kMaxReverseOutput
Output voltage limits to protect hardware and avoid saturation.
Current Limits
Protect motors by limiting supply and stator currents.
kNeutralMode
Sets motor neutral behavior (Brake or Coast).
Encoder Constants
Conversion factors and ports for accurate sensor feedback.
Homing Parameters
Controls the homing routine to safely find a reference position.
How to Tune the PID and Feedforward Constants
Proper tuning is critical to achieving smooth, accurate, and reliable motor control. The recommended tuning process is:
1. Find the Gravity Feedforward Constant, kG
Determine if your mechanism's gravity effect on the motor output is linear or follows a cosine curve.
Choose the appropriate model based on the mechanism’s geometry.
Increase kG until the subsystem feels weightless — that is, the motor output compensates exactly for gravity without moving the mechanism.
2. Use Characterize.java to Obtain Feedforward Values
Characterize.java to Obtain Feedforward ValuesRun the
Characterize.javarobot program to collect system data.Extract feedforward constants kS (static friction), kV (velocity), and kA (acceleration) from the characterization data.
Update your subsystem constants accordingly.
3. Add a Small Proportional Gain, kP
Start with a modest kP value to correct position or velocity errors.
Increase gradually until the system responds promptly without oscillating.
4. Begin with Conservative Second-Degree Motion Profiling
Start with slow cruise velocities and accelerations in the motion profile.
This ensures smooth and safe movement during tuning.
5. Gradually Increase Velocity and Acceleration
Raise cruise velocity and acceleration parameters until desired mechanism speed and responsiveness are achieved.
Observe for overshoot, instability, or excessive vibrations.
6. Add Derivative Gain, kD, to Reduce Overshoot
Introduce a small kD gain to dampen overshoot and oscillations.
Adjust as needed to smooth the stopping behavior of the mechanism.
You almost never should use integral, only use case is EXTREME accuracy (Madtown hasn't used it since 2022)
Last updated