Introduction
PID (Proportional, Integral, Derivative) controllers are the workhorses of control systems. They’re used in everything from temperature control in your home to autonomous drones and industrial robots. In this post, we’ll explore a particularly interesting implementation: a PID controller with velocity scaling, allowing the controller to automatically adapt to different speeds.
Imagine you’re driving a car. At low speeds, you make gentle steering adjustments, but at highway speeds, even a small turn of the wheel creates a significant change in direction. Our PID controller works similarly—it automatically adjusts its behavior based on how fast our system is moving.
What Is a PID Controller?
Before diving into the code, let’s understand what a PID controller does. At its core, a PID controller tries to minimize the difference (error) between a desired value (setpoint) and the actual measured value by adjusting an output.
Think of it like this:
- P (Proportional): How far are we from where we want to be? If we’re far, apply a bigger correction.
- I (Integral): Have we been off target for a while? If so, gradually increase correction.
- D (Derivative): Are we approaching our target quickly? If so, start easing off the correction to prevent overshooting.
Code Breakdown
Now, let’s examine our velocity-scaling PID controller implementation:
Class Definition
class PIDController {
public:
PIDController(const double setpoint, const double min, const double max,
const double kp, const double ki, const double kd,
const uint32_t minDt = 0, const double maxi = infinity(),
const double maxSetpointChange = infinity());
// Update controller
double advance(const double input, const double scaler = 1.0,
const bool isAngle = false);
void reset();
// Update parameters
void updateSetpoint(const double value);
void updateLimits(const double min, const double max);
void updateGains(const double kp, const double ki, const double kd);
void debugPrint(const char *name = nullptr, Stream &serial = Serial);
double currentSetpoint() const { return _setpoint; }
private:
// Parameters and internal state variables...
};
The constructor takes several parameters:
setpoint: The desired target valueminandmax: Limits for the controller outputkp,ki,kd: The PID gain coefficientsminDt: Minimum time between updates (prevents overly frequent updates)maxi: Maximum integral windup limitmaxSetpointChange: Maximum rate at which the setpoint can change
Constructor Implementation
PIDController::PIDController(const double targetSetpoint, const double min,
const double max, const double kp, const double ki,
const double kd, const uint32_t minDt,
const double maxi, const double maxSetpointChange)
: _targetSetpoint(targetSetpoint), _min(min), _max(max), _kp(kp), _ki(ki),
_kd(kd), _minDt(minDt), _maxi(maxi == infinity() ? maxi : maxi / ki),
_maxSetpointChange(maxSetpointChange) {}
The constructor initializes all the controller parameters. Note how it handles the _maxi parameter, which controls integral windup. If a maximum integral term is specified, it’s divided by ki to convert it to the internal representation.
The Core Algorithm: advance()
double PIDController::advance(const double input, const double scaler,
const bool isAngle) {
// Handle first iteration and invalid inputs
if (_justStarted) {
_justStarted = false;
return 0;
}
if (std::isnan(input)) return 0;
// Move setpoint towards target (with rate limiting)
const auto dsetpoint = constrain(_targetSetpoint - _setpoint,
-_maxSetpointChange, _maxSetpointChange);
_setpoint += dsetpoint;
// Check if enough time has passed since last update
if (micros() - _lastTime < _minDt) return _lastOutput;
// Calculate time delta
const auto now = micros();
const auto dt = now - _lastTime;
_lastTime = now;
// Calculate error
auto error = _setpoint - input;
// Update integral term with anti-windup
_integral += error * dt;
_integral = constrain(_integral, -_maxi, _maxi);
// Calculate PID terms with velocity scaling
const auto p = (_kp * scaler) * error;
const auto i = (_ki * _kp / dt * scaler) * _integral;
const auto d =
(_kd * _kp * dt * powf(scaler, 2)) * (error - _lastError) / dt;
// Combine and constrain output
const auto output = constrain(p + i + d, _min, _max);
// Store values for next iteration and debugging
_lastInput = input;
_lastP = p;
_lastI = i;
_lastD = d;
_lastDt = dt;
_lastOutput = output;
_lastError = error;
return output;
}
This is where the magic happens. Let’s break it down:
- Initial checks: Skip calculation on first call or with invalid input
- Setpoint ramping: Gradually move the current setpoint toward the target setpoint
- Time management: Ensure enough time has passed between updates
- Error calculation: Find the difference between setpoint and current value
- Integral update: Add to the accumulated error, with anti-windup protection
- PID calculations with velocity scaling: Apply the proportional, integral, and derivative terms with scaling
- Output limiting: Ensure the output stays within bounds
- State updates: Store values for the next iteration
Velocity Scaling
The most interesting aspect of this implementation is how it handles velocity scaling through the scaler parameter:
const auto p = (_kp * scaler) * error;
const auto i = (_ki * _kp / dt * scaler) * _integral;
const auto d = (_kd * _kp * dt * powf(scaler, 2)) * (error - _lastError) / dt;
This scaling allows the controller to automatically adjust its behavior based on system velocity:
- P term: Scales linearly with velocity (scaler)
- I term: Also scales linearly
- D term: Scales with the square of velocity (scaler²)
Think of this like steering sensitivity in a car that automatically adjusts with speed. At high speeds, the proportional and integral terms increase linearly, but the derivative term increases exponentially to prevent oscillations.
Example Decalration Of The Scaler
void Movement::drive() {
// Scale the controller output linearly to velocity with a reference
// velocity of 300 (we tune it at that)
auto scaler = velocity == 0 ? HEADING_STATIONARY_SCALER : velocity / 300;
const auto angularVelocity =
headingController.advance(actualHeading, scaler, true);
const auto angular = 0.25 * angularVelocity;
}
Supporting Functions
void PIDController::reset() {
_integral = 0.0F;
_lastError = 0.0F;
_lastTime = 0;
_lastOutput = 0.0F;
_justStarted = true;
}
void PIDController::updateSetpoint(const double value) {
if (!std::isnan(value)) _targetSetpoint = value;
}
void PIDController::updateLimits(const double min, const double max) {
_min = min;
_max = max;
}
void PIDController::updateGains(const double kp, const double ki,
const double kd) {
_kp = kp;
_ki = ki;
_kd = kd;
}
These utility functions allow for:
- Resetting the controller state
- Updating the target setpoint
- Changing the output limits
- Modifying the PID gains during operation
Debugging Support
void PIDController::debugPrint(const char *name, Stream &serial) {
const auto printdouble = [&serial](const double value) {
serial.printf("%5d.%02d", (int)value, abs((int)(value * 100) % 100));
};
if (name != nullptr) serial.printf("[%s] ", name);
serial.printf("Setpoint: ");
printdouble(_targetSetpoint);
// ... more debug output ...
}
The debugPrint method provides comprehensive debugging information, printing all the internal state to a serial port. This is invaluable during tuning and troubleshooting.
Key Features and Design Decisions
1. Anti-Windup Protection
The integral term in PID controllers can accumulate error indefinitely, causing “windup” that leads to large overshoots. Our implementation prevents this:
_integral += error * dt;
_integral = constrain(_integral, -_maxi, _maxi);
This constrains the integral to a maximum value, preventing runaway accumulation.
So you might be wondering what if I do not control my windup?
Imagine your drone with its altitude controller. The PID controller has a setpoint of 300m, but you’re holding it on the ground at 0m. Here’s what happens without windup protection:
- The controller sees a massive error: 300m - 0m = 300m
- The P term immediately wants to apply maximum thrust
- The D term is initially zero (no change in error yet)
With each passing millisecond, that 300m error gets added to the integral sum. The drone’s motors are spinning frantically, but since you’re holding it, it can’t move upward. The integral term keeps growing and growing, like winding up a spring tighter and tighter.
Then the moment you release the drone:
- It has this enormous accumulated integral term
- The motors are already at maximum power from the P term
- The I term is now pushing the controller to maintain maximum thrust far longer than it should
- The drone rockets upward, overshooting the 300m target dramatically
- By the time the P term starts to apply negative correction (when it passes 300m), the accumulated I term is still commanding “go up, go up, go up!”
This is exactly like a wind-up toy that’s been wound too tightly - when released, it spins wildly out of control before eventually settling down.
Without anti-windup protection, this behavior creates:
- Dangerous overshoots
- Oscillations that take a long time to dampen
- Potential instability
- In extreme cases, system failure (a drone that can’t stabilize might crash)
The anti-windup protection in the code (constraining the integral to a maximum value) prevents this by essentially saying, “No matter how long the error persists, we’re going to limit how much ‘energy’ gets stored in the integral term.” It’s like having a safety mechanism that prevents the spring from being wound too tightly.
This is particularly important for systems like drones, robots, or any application where the actuators might temporarily be unable to affect the controlled variable - whether because of physical constraints (like you holding the drone), saturation limits (motors already at max power), or external disturbances (wind pushing against movement).
2. Setpoint Ramping
Instead of immediately jumping to a new setpoint, this controller can gradually transition:
const auto dsetpoint = constrain(_targetSetpoint - _setpoint,
-_maxSetpointChange, _maxSetpointChange);
_setpoint += dsetpoint;
This is like gradually pressing the accelerator in your car rather than slamming it down, providing smoother transitions.
3. Minimum Update Time
To prevent excessive updates that might lead to instability:
if (micros() - _lastTime < _minDt) return _lastOutput;
This ensures the controller doesn’t run too frequently, which is important for systems with fast sampling rates.
4. Special Handling for Angular Values
The code includes commented-out logic for handling angular values (like compass headings):
// Correct for errors that are angles
if (isAngle) {
// If it goes beyond ±180º ∓ 90º, we try correcting in the other
// direction instead of going all the way around
if (error > 270.0F) {
error = -90.0F;
reset();
} else if (error < -270.0F) {
error = 90.0F;
reset();
}
}
When working with angles, the shortest path between two points might involve crossing the 0°/360° boundary. This logic would help find that shortest path.
Do note that this is untested and use it at your own will.
Conclusion
This velocity-scaling PID controller is a powerful tool for systems that operate across different speed ranges. By automatically adjusting the control parameters based on velocity, it provides consistent performance whether your system is moving slowly or quickly.
The implementation includes many practical features like anti-windup protection, setpoint ramping, and minimum update time, making it robust for real-world applications. The debug output capabilities also make it easier to tune and troubleshoot.