2. Advanced Motor Control

Advanced motor control is crucial for precise robot movement in FRC. This section covers three key concepts: PID Control for feedback-based control, Motion Profiles for smooth trajectory following, and Motion Magic for automated smooth motion.

PID Control

PID (Proportional-Integral-Derivative) control is a feedback control system that continuously calculates an error value as the difference between a desired setpoint and a measured process variable, then applies a correction based on proportional, integral, and derivative terms.

The Three Components

Proportional (P)

• Purpose: Provides an output proportional to the current error
• Effect: Larger errors result in larger corrections
• Formula: P_out = Kp × error
• Characteristics:
  • Fast response to large errors
  • Can cause steady-state error
  • May cause oscillation if too high

Integral (I)

• Purpose: Eliminates steady-state error by accumulating past errors
• Effect: Provides correction based on the sum of recent errors
• Formula: I_out = Ki × Σ(error × dt)
• Characteristics:
  • Eliminates steady-state error
  • Can cause overshoot and instability if too high
  • Slow response

Derivative (D)

• Purpose: Predicts future errors based on the rate of change
• Effect: Provides damping to reduce overshoot and oscillation
• Formula: D_out = Kd × (d(error)/dt)
• Characteristics:
  • Reduces overshoot and oscillation
  • Improves system stability
  • Sensitive to noise

PID Implementation in WPILib

// Create a PID Controller
PIDController pidController = new PIDController(kP, kI, kD);

// Set the setpoint (desired position)
pidController.setSetpoint(desiredPosition);

// Calculate the output in your periodic method
public void periodic() {
    double currentPosition = encoder.getDistance();
    double output = pidController.calculate(currentPosition);

    // Apply the output to your motor
    motor.set(output);
}

PID Tuning Process

1. Start with P only: Set I and D to 0, increase P until oscillation occurs
2. Add D: Increase D to reduce oscillation and overshoot
3. Add I: Add small I value to eliminate steady-state error
4. Fine-tune: Adjust all three values for optimal performance

PID Performance

Different kP	Different kI	Different kD
Larger kP results in a faster response; smaller kP slows the response.	Larger kI helps eliminate steady-state error faster but may cause overshoot and instability; smaller kI slows the correction of steady-state error.	Larger kD reduces overshoot and oscillation but may make the system more sensitive to noise; smaller kD provides less damping.

Motion Profile

A motion profile defines how a mechanism should move from one position to another over time, specifying position, velocity, and acceleration at each point. This ensures smooth, controlled motion rather than abrupt starts and stops.

Types of Motion Profiles

Trapezoidal Profile (1B)

• Characteristics: Constant acceleration, constant velocity, constant deceleration
• Use case: Most common, good for general purpose movement
• Phases:
  1. Acceleration phase: Constant acceleration from rest
  2. Cruise phase: Constant maximum velocity
  3. Deceleration phase: Constant deceleration to rest

S-Curve Profile (1A)

• Characteristics: Gradual acceleration changes (jerk limitation)
• Use case: When smooth motion is critical
• Advantage: Reduces mechanical stress and vibration

Advanced Profile Concepts

Feedforward Control

Feedforward improves profile following by predicting the required control effort:

// Simple feedforward for linear motion
double feedforward = setpoint.velocity * kV + setpoint.acceleration * kA;

// Gravity compensation for arms
double gravityFF = Math.cos(Math.toRadians(angle)) * kG;

motor.setVoltage(pidOutput + feedforward + gravityFF);

Profile Constraints

Consider physical limitations when setting constraints:

// Example constraints for different mechanisms
TrapezoidalProfile.Constraints driveConstraints =
    new TrapezoidalProfile.Constraints(4.0, 3.0); // Fast, aggressive

TrapezoidalProfile.Constraints armConstraints =
    new TrapezoidalProfile.Constraints(1.0, 0.5); // Slow, smooth

TrapezoidalProfile.Constraints elevatorConstraints =
    new TrapezoidalProfile.Constraints(2.0, 1.5); // Medium speed

Motion Magic®

Quoted from CTRE Docs

Motion Magic® is a control mode that provides the benefit of Motion Profiling without needing to generate motion profile trajectory points. When using Motion Magic®, the motor will move to a target position using a motion profile, while honoring the user specified acceleration, maximum velocity (cruise velocity), and optional jerk.

The benefits of this control mode over “simple” PID position closed-looping are:

• Control of the mechanism throughout the entire motion (as opposed to racing to the end target position)
• Control of the mechanism’s inertia to ensure smooth transitions between setpoints
• Improved repeatability despite changes in battery load
• Improved repeatability despite changes in motor load

After gain/settings are determined, the robot controller only needs to periodically set the target position.

There is no general requirement to “wait for the profile to finish”. However, the robot application can poll the sensor position and determine when the motion is finished if need be.

Motion Magic® functions by generating a trapezoidal/S-Curve velocity profile that does not exceed the specified cruise velocity, acceleration, or jerk. This is done automatically by the motor controller.

Note

If the remaining sensor distance to travel is small, the velocity may not reach cruise velocity as this would overshoot the target position. This is often referred to as a “triangle profile”.

An S-Curve profile has the following advantaged over a trapezoidal profile:

• Reducing oscillation of the mechanism.
• Maneuver is more deliberate and reproducible.

Note

The jerk control feature, by its nature, will increase the amount of time a movement requires. This can be compensated for by increasing the configured acceleration value.

The following parameters must be set when controlling using Motion Magic®:

• Cruise Velocity - peak/cruising velocity of the motion
• Acceleration - controls acceleration and deceleration rates during the beginning and end of motion
• Jerk (optional) - controls jerk, which is the derivative of acceleration