Servo Controller (C++)

Control a servo motor with position commands, including proportional control, effort clamping, and runtime gain tuning via horus::Params. Suitable for robotic arms, pan-tilt heads, and gripper fingers.

Problem

You have a servo or position-controlled actuator that accepts effort commands (torque or voltage). You need to drive it to target positions smoothly, clamp the maximum effort to prevent hardware damage, and adjust gains at runtime without restarting the system.

How It Works

The controller implements a proportional position loop with velocity feedforward:

error  = target_position - current_position
effort = clamp(Kp * error + Kv * velocity_feedforward, -max_effort, max_effort)

The servo hardware moves in response to the effort command, and the controller reads the new position on the next tick. Two parameters dominate behavior:

Kp (Proportional gain) -- determines tracking aggression

  • Low Kp (0.5-1.0): slow, smooth motion with low overshoot. Good for camera gimbals where smooth video matters more than speed.
  • Medium Kp (2.0-5.0): responsive tracking. Good for robotic arm joints that need to reach targets within a few hundred milliseconds.
  • High Kp (10.0+): fast response but oscillation risk. Only use with derivative damping (see PID recipe).

max_effort -- protects hardware from self-destruction

Every servo has a torque or current limit beyond which gears strip, windings overheat, or mounting hardware deforms. The max_effort clamp is the last line of defense. Set it to 80% of the servo's rated maximum to leave margin for transient loads.

Runtime parameter tuning via horus::Params lets you adjust Kp, max_effort, and max_velocity while the robot is running. This is critical for field calibration where you cannot afford to stop the system, recompile, and redeploy for every gain change. Parameters are read every tick so changes take effect within 10 ms at 100 Hz.

Feedback timeout protects against encoder failures. If the position feedback topic goes silent for 2 seconds, the controller zeros effort rather than blindly driving the servo into a hard stop. This is a fail-safe: the servo coasts to a stop under its own friction rather than crashing at full torque.

When To Use

  • Position-controlled servos (hobby PWM servos, Dynamixel, industrial servo drives)
  • Pan-tilt camera heads with live tuning requirements
  • Gripper fingers that need adjustable grip force
  • Any single-DOF actuator needing smooth position tracking with hardware protection
  • Field-deployed robots where gains must be adjusted without restarting

Prerequisites

  • HORUS installed (Installation Guide)
  • Basic understanding of nodes and topics (C++ Quick Start)
  • A servo or actuator that accepts effort (torque/voltage) commands and provides position feedback

Complete Code

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

class ServoController : public horus::Node {
public:
    ServoController() : Node("servo_controller") {
        // Subscribe to position targets from upstream (planner, teleop, etc.)
        target_sub_   = subscribe<horus::msg::CmdVel>("servo.target");
        // Subscribe to actual position feedback from servo encoder
        feedback_sub_ = subscribe<horus::msg::CmdVel>("servo.feedback");
        // Publish effort commands to the servo driver
        motor_pub_    = advertise<horus::msg::ServoCommand>("servo.cmd");
    }

    void tick() override {
        // ── Read runtime parameters (can be changed without restart) ─
        double kp         = params_.get<double>("servo_kp", 2.0);
        double max_effort = params_.get<double>("servo_max_effort", 5.0);
        double max_vel    = params_.get<double>("servo_max_velocity", 2.0);  // rad/s
        double deadband   = params_.get<double>("servo_deadband", 0.005);    // rad

        // ── Read latest target and feedback ─────────────────────────
        // IMPORTANT: always recv() every tick to drain buffers even if
        // you don't use the value this tick.
        if (auto target = target_sub_->recv()) {
            target_pos_ = target->get()->linear;
        }
        if (auto feedback = feedback_sub_->recv()) {
            prev_pos_ = current_pos_;
            current_pos_ = feedback->get()->linear;
            has_feedback_ = true;
            no_feedback_count_ = 0;
        }

        // ── Safety: do not command without position feedback ────────
        if (!has_feedback_) {
            if (++no_feedback_count_ % 200 == 0) {  // warn every 2s at 100 Hz
                horus::log::warn("servo",
                    "No feedback for 2s — holding zero effort for safety");
            }
            horus::msg::ServoCommand cmd{};
            cmd.target_position = static_cast<float>(target_pos_);
            cmd.target_velocity = 0.0f;
            cmd.max_force = 0.0f;  // zero effort without feedback
            motor_pub_->send(cmd);
            return;
        }

        // ── Deadband: ignore errors smaller than encoder resolution ─
        double error = target_pos_ - current_pos_;
        if (std::abs(error) < deadband) {
            // Close enough — hold position with zero effort to save power
            horus::msg::ServoCommand cmd{};
            cmd.target_position = static_cast<float>(current_pos_);
            cmd.target_velocity = 0.0f;
            cmd.max_force = static_cast<float>(max_effort);
            motor_pub_->send(cmd);
            return;
        }

        // ── Proportional control ────────────────────────────────────
        double effort = std::clamp(kp * error,
                                   -max_effort, max_effort);

        // ── Velocity limiting ───────────────────────────────────────
        // Estimate current velocity from position delta
        double dt = 0.01;  // 100 Hz tick rate
        double measured_vel = (current_pos_ - prev_pos_) / dt;

        // Clamp effort to keep velocity under max_vel
        // If already moving faster than max_vel, reduce effort
        if (std::abs(measured_vel) > max_vel) {
            double brake = std::copysign(max_effort * 0.5, -measured_vel);
            effort = brake;  // actively brake if overspeeding
        } else {
            // Limit implied acceleration
            double implied_vel = std::abs(effort) / kp;
            if (implied_vel > max_vel) {
                effort = std::copysign(max_vel * kp, effort);
                effort = std::clamp(effort, -max_effort, max_effort);
            }
        }

        // ── Publish servo command ───────────────────────────────────
        horus::msg::ServoCommand cmd{};
        cmd.target_position = static_cast<float>(target_pos_);
        cmd.target_velocity = static_cast<float>(effort);
        cmd.max_force       = static_cast<float>(max_effort);
        motor_pub_->send(cmd);

        // ── Diagnostics at 2 Hz (every 50th tick at 100 Hz) ────────
        if (++tick_count_ % 50 == 0) {
            char buf[196];
            std::snprintf(buf, sizeof(buf),
                "target=%.3f  actual=%.3f  error=%.4f  effort=%.2f  "
                "vel=%.2f  kp=%.1f  max_eff=%.1f",
                target_pos_, current_pos_, error, effort,
                measured_vel, kp, max_effort);
            horus::log::info("servo", buf);
        }
    }

    void enter_safe_state() override {
        // Zero effort on safety event — servo coasts to stop under friction.
        // Holding position under power during an emergency is dangerous:
        // a jammed servo draws maximum current and can overheat or break
        // mechanical linkages.
        horus::msg::ServoCommand stop{};
        stop.target_position = static_cast<float>(current_pos_);
        stop.target_velocity = 0.0f;
        stop.max_force = 0.0f;
        motor_pub_->send(stop);

        horus::blackbox::record("servo",
            "Entered safe state, effort zeroed, servo coasting");
    }

    void set_params(horus::Params& p) { params_ = std::move(p); }

private:
    horus::Subscriber<horus::msg::CmdVel>*     target_sub_;
    horus::Subscriber<horus::msg::CmdVel>*     feedback_sub_;
    horus::Publisher<horus::msg::ServoCommand>* motor_pub_;
    horus::Params params_;

    // State
    double target_pos_  = 0;
    double current_pos_ = 0;
    double prev_pos_    = 0;
    bool has_feedback_  = false;
    int no_feedback_count_ = 0;
    int tick_count_ = 0;
};

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

    // Set initial parameters (adjustable at runtime via `horus param set`)
    horus::Params params;
    params.set("servo_kp", 2.0);           // proportional gain
    params.set("servo_max_effort", 5.0);   // Nm (set to 80% of servo rated max)
    params.set("servo_max_velocity", 2.0); // rad/s
    params.set("servo_deadband", 0.005);   // rad (~0.3 degrees)

    ServoController servo;
    servo.set_params(params);

    sched.add(servo)
        .order(10)
        .budget(2_ms)
        .on_miss(horus::Miss::Skip)  // skip if overrun — stale effort is dangerous
        .build();

    sched.spin();
}

Gain Tuning Guide

Start with low Kp (0.5). Command a small step (e.g., 0.1 rad) and watch the response:

  • Slow, never reaches target: increase Kp. Double it until the servo reaches the target within 0.5 seconds.
  • Reaches target but slowly: Kp is close. Increase by 20% increments.
  • Overshoots then settles: Kp is at the edge. Back off 30% or add derivative damping (see PID recipe).
  • Oscillates continuously: Kp is too high. Halve it.

Set max_effort conservatively. Read the servo datasheet for stall torque. Set servo_max_effort to 60-80% of stall torque. A Dynamixel MX-64 has 6.0 Nm stall torque, so max_effort = 4.0 is a safe starting point.

Set max_velocity for the application:

ApplicationTypical max_vel
Camera gimbal0.5-1.0 rad/s
Robotic arm joint1.0-3.0 rad/s
Gripper finger0.2-0.5 rad/s
Pan-tilt turret1.5-4.0 rad/s

Runtime tuning via CLI (no restart needed):

horus param set servo_kp 3.5
horus param set servo_max_effort 4.0
horus param set servo_max_velocity 1.5

Common Errors

SymptomCauseFix
Servo oscillates around targetKp too high for the mechanical systemReduce servo_kp by 50%; add derivative damping (see PID recipe)
Servo moves slowly, never reaches targetKp too low or max_effort too lowIncrease servo_kp or servo_max_effort; check for mechanical binding
Servo jerks on large target changesVelocity limiting too permissiveReduce servo_max_velocity to limit tracking speed
Servo does nothingNo feedback data (has_feedback_ stays false)Verify feedback publisher topic name matches servo.feedback; check encoder wiring
Effort maxes out constantlyTarget far from current positionWorking correctly but servo needs more travel time; reduce target step size or increase max_effort
Servo holds position but drifts slowlyDeadband too largeReduce servo_deadband (default 0.005 rad is ~0.3 degrees)
Servo chatters near targetDeadband too small relative to encoder noiseIncrease servo_deadband to 2x the encoder's noise floor
Servo runs away after encoder disconnectFeedback timeout not triggeringThe code handles this: zero effort after 200 ticks (2s) without feedback
Log says "No feedback for 2s" at startupFeedback publisher not started yetNormal during system bringup; servo holds zero effort until feedback arrives

Design Decisions

ChoiceRationale
horus::Params for runtime tuningField calibration requires gain changes without restart; impossible to predict optimal gains for every installation
Position feedback required before commandingOpen-loop servo control risks collisions and mechanical damage; the controller refuses to move without knowing where it is
Feedback timeout (2s)If the encoder dies mid-operation, zero effort prevents the servo from driving into a hard stop at maximum torque
enter_safe_state() zeros effortHolding position under power during an emergency risks overheating, stripped gears, or broken linkages if the servo is jammed
Miss::Skip policyA stale effort command computed from an old position reading drives the servo based on outdated state, which is worse than one missed tick
Deadband around targetEliminates chatter near the setpoint caused by encoder quantization noise; saves power when the servo is already at the target
Velocity limiting via measured velocityPrevents jerky motion on large target step changes and actively brakes if the servo is overspeeding
100 Hz tick rateStandard for servo control: fast enough for smooth motion (10 ms latency), slow enough for most encoder hardware
order(10) with 2 ms budgetServo control runs early in the tick (before logging/diagnostics) with a tight budget to guarantee real-time behavior

Variations

Multi-joint arm control -- control N servos as a coordinated unit:

class ArmController : public horus::Node {
    static constexpr int NUM_JOINTS = 6;
    horus::Subscriber<horus::msg::JointState>* target_sub_;
    horus::Subscriber<horus::msg::JointState>* feedback_sub_;
    horus::Publisher<horus::msg::JointState>*  cmd_pub_;
    double positions_[NUM_JOINTS] = {};
    double kp_[NUM_JOINTS] = {3.0, 2.5, 2.5, 4.0, 4.0, 5.0};  // per-joint gains
    double max_effort_[NUM_JOINTS] = {6.0, 5.0, 5.0, 3.0, 3.0, 2.0};  // Nm per joint

    void tick() override {
        if (auto fb = feedback_sub_->recv()) {
            for (int i = 0; i < NUM_JOINTS; i++)
                positions_[i] = fb->get()->position[i];
        }
        if (auto target = target_sub_->recv()) {
            horus::msg::JointState cmd{};
            for (int i = 0; i < NUM_JOINTS; i++) {
                double error = target->get()->position[i] - positions_[i];
                cmd.effort[i] = std::clamp(kp_[i] * error,
                    -max_effort_[i], max_effort_[i]);
            }
            cmd_pub_->send(cmd);
        }
    }
};

Trajectory following -- interpolate between waypoints for smooth, jerk-free motion:

void tick() override {
    double t_elapsed = (tick_count_ - segment_start_) * 0.01;  // seconds
    double t_norm = std::clamp(t_elapsed / segment_duration_, 0.0, 1.0);

    // Cubic interpolation (zero velocity at endpoints)
    double s = 3.0 * t_norm * t_norm - 2.0 * t_norm * t_norm * t_norm;
    target_pos_ = start_pos_ + s * (end_pos_ - start_pos_);

    // ... rest of proportional control as above
}

PID instead of P-only -- for faster convergence and zero steady-state error under load:

double error = target_pos_ - current_pos_;
double dt = 0.01;
integral_ += error * dt;
integral_ = std::clamp(integral_, -0.5, 0.5);  // anti-windup
double derivative = (error - prev_error_) / dt;
double effort = kp * error + ki * integral_ + kd * derivative;
prev_error_ = error;

Gravity compensation -- for vertical joints that fight gravity:

// Add a constant feedforward term to counteract gravity torque.
// Calibrate by measuring the effort needed to hold the joint still.
double gravity_comp = 1.2;  // Nm, measured experimentally
double effort = kp * error + gravity_comp * std::cos(current_pos_);

Key Takeaways

  • Always require position feedback before commanding effort -- open-loop servo control is dangerous and risks mechanical damage
  • horus::Params enables runtime tuning without restarting -- essential for field calibration where stopping the system is not an option
  • Deadband eliminates chatter near the target and saves power by not fighting encoder noise
  • Velocity limiting prevents jerky motion on large target changes and actively brakes overspeeding servos
  • enter_safe_state() should zero effort, not hold position -- powering the motor during an emergency creates fire and mechanical failure risks
  • Miss::Skip is correct for servo control because a stale error computation drives the servo based on an outdated position reading
  • Set max_effort to 60-80% of the servo's rated stall torque -- leave margin for transient loads