PID Controller (C++)

A reusable PID controller node that reads a setpoint and measured value, then publishes a control output. Includes integral anti-windup and derivative low-pass filtering.

Problem

You need a closed-loop controller that drives a measured value toward a target setpoint with tunable response characteristics.

How PID Works

A PID controller is a feedback loop. It continuously measures the difference between where you want to be (setpoint) and where you are (measured value), then computes a control output to close that gap.

P (Proportional) — reacts to the present error

P = Kp * error

Large error produces large output. P alone gets you close but usually leaves steady-state error — at some point the output is too small to overcome friction.

I (Integral) — eliminates steady-state error

I = Ki * sum_of_errors_over_time

The integral accumulates past errors. Even tiny current error builds up until the system reaches setpoint exactly. Risk: if the actuator saturates, the integral keeps growing (windup). That's why the code clamps integral to integral_max_.

D (Derivative) — reduces overshoot

D = Kd * rate_of_change_of_error

Looks at how fast error is changing. If error is shrinking rapidly, D produces a braking force preventing overshoot. Downside: differentiating a noisy sensor amplifies noise, so the code applies a low-pass filter.

Combined output:

output = clamp(P + I + D, -1.0, 1.0)

When To Use

Position, velocity, or temperature control loops
Any system with a measurable output and controllable input
When you need tunable response without a plant model

Prerequisites

HORUS installed (Installation Guide)
Basic understanding of nodes and topics (C++ Quick Start)

Complete Code

#include <horus/horus.hpp>
#include <algorithm>
#include <cmath>
#include <cstdio>
using namespace horus::literals;

class PidController : public horus::Node {
public:
    PidController(double kp, double ki, double kd)
        : Node("pid_controller"), kp_(kp), ki_(ki), kd_(kd)
    {
        setpoint_sub_    = subscribe<horus::msg::CmdVel>("pid.setpoint");
        measurement_sub_ = subscribe<horus::msg::CmdVel>("pid.measurement");
        output_pub_      = advertise<horus::msg::CmdVel>("pid.output");
    }

    void tick() override {
        // IMPORTANT: always recv() every tick to drain buffers
        if (auto sp = setpoint_sub_->recv()) {
            target_ = sp->get()->linear;
        }
        if (auto fb = measurement_sub_->recv()) {
            measured_ = fb->get()->linear;
        }

        double error = target_ - measured_;
        double dt = 0.01;  // 100 Hz tick rate

        // ── Proportional ────────────────────────────────────────
        double p_term = kp_ * error;

        // ── Integral with anti-windup ───────────────────────────
        integral_ += error * dt;
        integral_ = std::clamp(integral_, -integral_max_, integral_max_);
        double i_term = ki_ * integral_;

        // ── Derivative with low-pass filter ─────────────────────
        // Raw derivative amplifies sensor noise.
        // Filter: alpha * prev + (1 - alpha) * new
        double raw_derivative = (error - prev_error_) / dt;
        filtered_derivative_ = alpha_ * filtered_derivative_
                             + (1.0 - alpha_) * raw_derivative;
        double d_term = kd_ * filtered_derivative_;
        prev_error_ = error;

        // ── Output (clamped to actuator limits) ─────────────────
        double output = std::clamp(p_term + i_term + d_term,
                                   output_min_, output_max_);

        // Publish with diagnostic fields
        horus::msg::CmdVel cmd{};
        cmd.linear = static_cast<float>(output);
        cmd.angular = static_cast<float>(error);  // expose error for monitoring
        output_pub_->send(cmd);

        // Log every 100 ticks (1 Hz at 100 Hz)
        if (++tick_count_ % 100 == 0) {
            char buf[128];
            std::snprintf(buf, sizeof(buf),
                "err=%.3f P=%.3f I=%.3f D=%.3f out=%.3f",
                error, p_term, i_term, d_term, output);
            horus::log::info("pid", buf);
        }
    }

    void enter_safe_state() override {
        // Zero output on safety event — stop actuator
        horus::msg::CmdVel stop{};
        output_pub_->send(stop);
        integral_ = 0;  // reset integral to prevent windup during recovery
        horus::blackbox::record("pid", "Entered safe state, output zeroed");
    }

private:
    horus::Subscriber<horus::msg::CmdVel>* setpoint_sub_;
    horus::Subscriber<horus::msg::CmdVel>* measurement_sub_;
    horus::Publisher<horus::msg::CmdVel>*  output_pub_;

    // Gains
    double kp_, ki_, kd_;

    // State
    double integral_ = 0;
    double prev_error_ = 0;
    double filtered_derivative_ = 0;
    double target_ = 0;
    double measured_ = 0;
    int tick_count_ = 0;

    // Limits
    double output_min_ = -1.0;
    double output_max_ = 1.0;
    double integral_max_ = 0.5;   // anti-windup clamp
    double alpha_ = 0.8;          // derivative filter (0=no filter, 0.99=heavy)
};

int main() {
    horus::Scheduler sched;
    sched.tick_rate(100_hz).name("pid_demo");

    PidController pid(1.0, 0.1, 0.05);  // Kp=1, Ki=0.1, Kd=0.05
    sched.add(pid)
        .order(10)
        .budget(5_ms)
        .on_miss(horus::Miss::Skip)  // skip if overrun — don't accumulate lag
        .build();

    sched.spin();
}

Tuning Guide

Start with P only (Ki=0, Kd=0). Increase Kp until the system oscillates, then back off 50%.

Add I to eliminate steady-state error. Start small (Ki = Kp/10). If the system overshoots and oscillates slowly, reduce Ki.

Add D to reduce overshoot. Start with Kd = Kp/5. If the output is noisy, increase alpha_ (heavier filter).

Ziegler-Nichols method:

Set Ki=0, Kd=0
Increase Kp until sustained oscillation (this is Ku)
Measure oscillation period Tu
Set: Kp = 0.6 * Ku, Ki = 2 * Kp / Tu, Kd = Kp * Tu / 8

Common Errors

Symptom	Cause	Fix
Oscillation that grows	Kp too high	Reduce Kp by 50%
Slow convergence	Kp too low or Ki too low	Increase Kp or Ki
Overshoot then settle	Kd too low	Increase Kd
Noisy output	Kd amplifying sensor noise	Increase alpha (0.9+) or reduce Kd
Windup after saturation	integral_max too large	Reduce integral_max to match actuator range
Output stuck at ±1.0	Sustained error with high Ki	Reduce Ki or increase integral_max clamp

Design Decisions

Choice	Rationale
`horus::Node` subclass	State (integral, prev_error) persists across ticks cleanly
`enter_safe_state()` zeros output	Actuator stops immediately on safety event
Derivative filter	Raw `d/dt(error)` amplifies sensor noise — low-pass essential
Integral clamp	Prevents windup when actuator is saturated
`Miss::Skip` policy	PID with accumulated lag is worse than skipping a tick
100 Hz tick rate	Fast enough for motor control, slow enough for most sensors

Variations

Velocity PID — for motor speed control, compute on velocity error instead of position:

double error = target_velocity - measured_velocity;

Cascaded PID — outer loop sets inner loop's setpoint:

// Outer: position → velocity setpoint
// Inner: velocity → motor command
double vel_setpoint = position_pid.compute(pos_error);
double cmd = velocity_pid.compute(vel_setpoint - measured_vel);

Runtime gain tuning with horus::Params:

void tick() override {
    kp_ = params_.get<double>("pid_kp", kp_);
    ki_ = params_.get<double>("pid_ki", ki_);
    kd_ = params_.get<double>("pid_kd", kd_);
    // ... rest of PID computation
}

Key Takeaways

Always drain subscriber buffers every tick (recv() unconditionally)
Use enter_safe_state() for safety-critical actuator control
Anti-windup is mandatory for any real PID — integral grows unbounded without it
Filter the derivative — raw differentiation amplifies noise
Miss::Skip is better than Miss::Warn for control loops — accumulated lag causes oscillation