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.

I recommend watching this video for a conceptual understanding: https://www.youtube.com/watch?v=JEpWlTl95Tw

---

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

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

• Run the Characterize.java robot 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)