Skip to content

Motion Profiling

PIDs

PID control is a common way to control a motor to reach a specific setpoint.

It uses three terms to determine how much power to apply to the motor: Proportional, Integral, and Derivative.

  • Proportional (P) is the amount of power applied based on the error from the setpoint.
  • Integral (I) is the amount of power applied based on the accumulated error over time.
  • Derivative (D) is the amount of power applied based on the rate of change of the error.

For a more exact explanation of how they work, visit this article. I also think this video does a good job of showing what each term does.

Feedforward

Feedforward is a way to apply a constant amount of power to a motor based on the desired velocity and acceleration. This allows the motor to reach the desired velocity and acceleration more quickly, without having to rely solely on PID control. However, a PID should almost always be used on top of feedforward in FRC for precision.

Feedforward often uses four terms: kS, kV, kA, and kG.

  • kS is a static voltage that is always applied to the motor whenever the output != 0; helps to overcome friction.
  • kV is the voltage required to achieve 1 rotation per second of velocity.
  • kA is the voltage required to achieve 1 rotation per second squared of acceleration.
  • kG is the voltage required to overcome gravity.

For a more exact explanation of how they work, visit this article.

Motion Magic

CTRE motors use Motion Magic, which is a control setup that is outside of the normal RoboRIO 20ms loop. This is useful because it provides a more precise motion profile for constant setpoints. There are many constraints that you can add to Motion Magic, allowing you to have fine-tuned subsystem control.

This is the general constraint setup for a Motion Magic Motor's constraints: 

val motionMagicConfig = TalonFXConfiguration().apply {
    MotorOutput.NeutralMode = NeutralModeValue.Brake // Can be Brake or Coast mode.
    MotorOutput.Inverted = InvertedValue.Clockwise_Positive // This determines if the motor outputs and inputs should be inverted or not.
    Feedback.SensorToMechanismRatio = Constants.Subsystem.GEAR_RATIO // This will convert inputs and outputs to match a specific ratio. Ex: 5.0 sensor to mechanism ratio will read 5 shaft rotations as one mechanism rotation.
    Slot0.kS = 0.0 // This is a static voltage that is always applied to the motor whenever the output != 0; helps to overcome friction.
    Slot0.kV = 0.0 // The voltage required to achieve 1 rotation per second of velocity.
    Slot0.kA = 0.0 // The voltage required to achieve 1 rotation per second squared of acceleration.
    Slot0.kG = 0.0 // The voltage required to overcome gravity.
    Slot0.kP = 0.0 // The proportional term of a Motion Magic PID.
    Slot0.kI = 0.0 // The integral term of a Motion Magic PID.
    Slot0.kD = 0.0 // The derivative term of a Motion Magic PID.
    MotionMagic.MotionMagicJerk = 9999.0 // Target Motion Magic Jerk (Jerk is the acceleration of acceleration), generally can be ignored, but if a mechanism is weirdly jittery, reducing this may help.
    MotionMagic.MotionMagicAcceleration = 0.0 // Target Motion Magic acceleration.
    MotionMagic.MotionMagicCruiseVelocity = 0.0 // Target Motion Magic velocity.
    // Current limiting: Stator current affects acceleration capability, while Supply current helps prevent breaker trips.
    // Refer to the CTRE docs for detailed information on current limiting.
    CurrentLimits.StatorCurrentLimit = 40.0  // This should be more than 40 for a drivetrain, can go decently high, but too high may kill battery life.
    CurrentLimits.SupplyCurrentLimit = 40.0 // Probably also want this to be >40 for a drivetrain, but not much higher.
}
You can view more at the CTRE docs.

Tuning a Subsystem

When using feedforward and PID control at the same time, the PID acts as a correction to any feedforward error, so it should be tuned second. Not all subsystems use feedforward and PID control; sometimes raw PID is enough, but using both is generally best. The following is what I recommend for each subsystem, though you may choose whatever control method you want:

Subsystem Type PID Control Feedforward Control
Swerve (Drive Motors) Yes Yes
Swerve (Azimuth Motors) Yes Optional
Arm Yes Yes
Elevator Yes Yes
Intake pivot Yes Optional
Flywheels Optional Yes
Rollers Optional Optional

The general process for tuning a subsystem is as follows:

  1. Find kG (if applicable). This is done by slowly increasing kG until the mechanism can hold its position without any input. On a rotating mechanism, this is done at the horizontal position.
  2. Obtain data for kV, kA, and kS. This is done by running the mechanism at various voltages, starting low, then recording the velocity and acceleration at each voltage. You can then use this data to calculate kV, kA, and kS using linear regression. You may also use SysID to calculate these values for you, but I have not had much success with it.
  3. Tune the P value. Start with a low value and increase it until the mechanism can reach the setpoint quickly without overshooting significantly.
  4. Tune the D value. Increase the D value until the mechanism stops oscillating around the setpoint. It should be a small value.
  5. Tune the I value (rarely should be used, typically only causes issues unless you know what you're doing). Increase the I value until the mechanism can hold its position without drifting. It should be a small value.