Map ToF distance to a servo angle without jitter

You have a time-of-flight ranging module streaming millimeters over I2C and a servo that should follow the target: 40 mm at 0°, 4000 mm at 180°. The sketch compiles, the readings look sane, and the servo jerks or sweeps instead of tracking. Two fixes carry nearly every build like this: the arithmetic — one integer line that scales and clamps the range — and the power — a servo fed from a rail it cannot brown out. What follows is the formula, the deadband that stops hunting, the wiring that stops jitter, a checkpoint table you can test with a tape measure, and the cases where an open-loop angle is the wrong design.

How do I turn a ToF distance into a servo angle?

Subtract the minimum range, scale by the span, then clamp: angle = (distance - 40 mm) × 180 ÷ 3960, which is 22 mm of range per degree of servo travel, or 0.045° per millimeter.

The two constants are your calibration points: 40 mm maps to 0°, 4000 mm maps to 180°, and everything between sits on a straight line. The span is 4000 - 40 = 3960 mm, so 3960 ÷ 180 = 22 mm buys one degree. Integer arithmetic is exact enough here — the servo resolves single degrees, so floating point buys nothing.

The map() helper used in most servo examples computes the same integer division: map(reading, 40, 4000, 0, 180). It is a plain straight line with no limits of its own, so an input outside the declared range returns an angle outside 0-180°. The clamp is yours to add, and the next section shows why it is not optional.

Worked example, target at 1500 mm: (1500 - 40) = 1460; 1460 × 180 = 262,800; 262,800 ÷ 3960 = 66.4°, which integer division truncates to 66°. The discarded 0.4° is 8 mm of range — inside the 22 mm the servo already quantizes to. Under the hood the servo library turns each degree into a pulse width: a standard RC servo spans about 1000-2000 µs across 0-180°, roughly 5.6 µs per degree, so single-degree commands land cleanly.

Scaling one sensor's numbers onto another device's range is the same job an image to map homography does for pixel positions onto ground-plane meters: fix two calibration points, keep the transform linear, clamp what falls outside them.

Why does the servo sweep to its end stops when the target leaves the window?

Because the straight-line map keeps running past the end stops: a reading below 40 mm computes a negative angle and a saturated no-target reading lands past 180°, so alternating valid and invalid readings look like full 0-180° sweeps.

A ranging module that loses its target does not send silence. It returns a failed acquisition as zero or as a pinned out-of-range value, and the mathematics stays 'correct' all the way through the map. Swap between a real target and a lost one at 100 readings per second and the servo thrashes between the rails.

Guard both ends, in this order. Reject the reading before mapping: a range of zero, or the module's documented no-target value, means hold the last commanded angle and skip the cycle. Clamp after mapping: below 0° becomes 0°, above 180° becomes 180°. One line each, and a data glitch becomes a stationary pointer instead of a full sweep.

One physical limit to respect: these modules have a minimum standoff, and a target closer than that floor returns merged pulses. Keep the 40 mm lower bound at or above the module's own floor.

Why does the servo jitter even when the code is correct?

Because of the stall current — the draw while the servo is held against its load, an order of magnitude above idle — which collapses the shared 5 V rail for the milliseconds the pulse train needs, and the sensor resets or misreads.

The bench symptoms are recognizable: the servo twitches, the board's indicator flickers in step with it, the module drops off the I2C bus and re-initializes, the USB link dies during a long sweep. All point at one rail. A logic regulator and a USB port were sized for chips, never for a motor at stall.

Size the servo rail from stall, not idle. Example input: a servo whose data sheet lists 0.8 A stall at 6 V. That is 0.8 × 6 = 4.8 W the rail must hold while stalled, so take the next converter class up — 10 W — and the margin covers start-up transients. Substitute your data-sheet figures; the arithmetic is unchanged.

On a machine, feed a step-down converter from the 24/48 V DC rail and let the logic and servo supplies meet only at ground. The LRS-75-48 is an enclosed supply rated 48 V and 76.8 W.

Bench builds get the same result from any separate 5-6 V source with its own current budget. The power supplies category lists the enclosed units, and the triage order for a device that resets when an actuator fires is walked through in camera GPIO wiring: prevent damage and triage power faults.

How much hysteresis does the servo need to stop hunting?

A 2° deadband — 44 mm of range, since one degree costs 22 mm — plus a five-reading average removes visible hunting on a bench-sized target; tighten it only when the pointer must resolve finer motion.

Hunting starts at the bottom of the signal chain. Ranging scatter and target texture put millimeter-level variation on consecutive readings, and after the map each variation is a fraction of a degree. That is invisible alone, but small alternating commands arrive faster than the horn can settle, and the result reads as nervous twitching.

Two settings stop it, and they are independent. Average five consecutive valid readings before mapping: at a 10 ms measurement period that is 50 ms of lag, and it removes most of the scatter. Then gate the write: send a new command only when the target differs from the last one written by 2° or more, so under 44 mm of change nothing moves. If the pointer still flutters on a noisy surface, widen to 3° (66 mm) rather than averaging harder.

How should power and ground be wired to keep the rail clean?

Give the servo its own 5-6 V rail from a separate supply or a step-down from the machine's 24/48 V DC, tie that rail's ground to logic ground at exactly one point, and put 470-1000 µF across the servo rail at the connector.

The details that decide whether the fix holds:

• Tie the grounds once. The servo pulse is referenced to ground; two rails with no common ground give a floating pulse the servo cannot read — the same fault class as an external trigger timeout from a wrong ground.
• Fit the bulk capacitor at the connector. It carries the first milliseconds of a stall while the converter's regulation loop catches up.
• Keep servo leads short, twisted, and away from the I2C pair. At one ampere, even 0.1 Ω of round-trip lead drops 0.1 V (V = I × R), and a 400 kHz bus tolerates little coupling.
• Drive the servo from a pin the board's pin map marks PWM-capable. Shared-pin conflicts are found on the map, not by trial — the trap is documented in camera shield shared IO pins.
• Out of timer pins? Drive servos from a vision board without PWM pins covers the alternatives.

How do I build the sketch step by step?

Configure the sensor once in setup(), then in loop read, validate, average, map, clamp, gate and write — in that order; each step carries one fix from the sections above, and skipping one puts the failure it prevented straight back into the servo.

1. Set the sensor up in setup() once: begin the I2C bus, set it to 400 kHz, point the ranging object at that bus, initialize. Configure long-distance mode, a 10 ms timing budget and a 10 ms inter-measurement period — about 100 readings per second.
2. Attach the servo object to one PWM-capable pin in setup(), never in loop(); re-attaching resets the timer and the pulse every cycle.
3. Read the range and validate: zero or the no-target value means hold the last angle and skip.
4. Accumulate five valid readings; use the average.
5. Map with the integer formula, then clamp to 0-180°.
6. Gate with the deadband: inside 2° of the last written angle, skip the write.
7. Write the angle and store it as the last written value.
8. Print range and angle together; the verification pass needs both columns.

// Illustrative sketch: ToF range (mm) -> servo angle (deg).
// Wrapper names are illustrative; swap in your library's calls.

const long RANGE_MIN_MM = 40;    // mm  maps to 0 deg
const long RANGE_MAX_MM = 4000;  // mm  maps to 180 deg
const int  DEADBAND_DEG = 2;     // deg, minimum change worth a write
const int  AVERAGE_N    = 5;     // valid readings per output

long range_mm   = 0;   // latest valid reading, mm
long sum_mm     = 0;   // running sum of the current average
int  n          = 0;   // readings in the current average
int  target_deg = 0;   // mapped and clamped angle
int  last_deg   = -1;  // last angle written, -1 = none yet

int mapRangeToAngle(long mm) {
  int deg = (int)((mm - RANGE_MIN_MM) * 180 / (RANGE_MAX_MM - RANGE_MIN_MM));
  if (deg < 0)   deg = 0;    // closer than 40 mm
  if (deg > 180) deg = 180;  // farther than 4000 mm
  return deg;
}

void loop() {
  range_mm = tofReadMm();        // illustrative wrapper, returns mm
  if (range_mm <= 0) return;     // no target: hold the last angle

  sum_mm += range_mm;
  n = n + 1;
  if (n < AVERAGE_N) return;

  target_deg = mapRangeToAngle(sum_mm / AVERAGE_N);
  sum_mm = 0;
  n = 0;

  if (last_deg >= 0 && abs(target_deg - last_deg) < DEADBAND_DEG) return;
  myservo.write(target_deg);     // servo object attached in setup()
  last_deg = target_deg;
}

The desk test for the mapping function, by hand or in a unit test:

// mapRangeToAngle(40)   -> 0    ((40 - 40) * 180 / 3960 = 0)
// mapRangeToAngle(2020) -> 90   ((2020 - 40) * 180 = 356400 / 3960)
// mapRangeToAngle(4000) -> 180  ((4000 - 40) * 180 = 712800 / 3960)
// mapRangeToAngle(30)   -> 0    clamped, raw value 0
// mapRangeToAngle(4500) -> 180  clamped, raw value 202

How do I verify the mapping before calling it done?

Tape-measure five checkpoints and compare the commanded angle against the table: 40 mm must read 0°, 2020 mm must read 90°, 4000 mm must read 180°, with one-degree steps of 22 mm between them.

Integer division truncates, so 44.5° arrives as 44° and 134.5° as 134° — that is quantization, not error. Then prove the electronics: put a current meter in the servo rail and sweep 0-180° under load; the rail must hold. If you have a scope, watch the module's supply during the sweep — any dip in step with servo motion is wiring, and no code hides it.

When is a linear distance-to-angle map the wrong design?

When the servo must place something rather than point at it: a 2° deadband is 44 mm of uncertainty, gear backlash adds its own error on top, and any job that picks, gates or measures needs feedback or a calibrated transform instead of an open-loop angle.

Close the loop, in one of two ways. An encoder on the axis reports what the horn actually did. Or a camera measures the result and commands corrections — the calibration step for that route is the one in convert image coordinates to robot coordinates for servo moves.

When the ranging side must grow — wider coverage, a depth image instead of a single beam, an industrial housing — the packaged options live in the 3D sensors category. A unit such as the MV-DT01WMU replaces the bare module, and the same map-clamp-deadband logic runs on the host.

Mechanics cap the electronics. A sensor or servo on a floppy bracket adds angle error no code removes, and the stillness rule from 3D camera mounting applies to a single beam as much as to a full frame: rigid mount, short levers, nothing that resonates when the servo reverses. The camera stands category carries brackets and posts; the configurator's stand step assembles the mounting list.

What should I check before committing to the build?

Work the list top to bottom: span with headroom at both end stops, a servo rail sized above stall current, one common ground, a confirmed PWM pin, clamp-reject-deadband in the code, and the tape-measure table passing at 40, 2020 and 4000 mm.

• Span with headroom. Keep the useful motion 10° inside each end stop, so the servo never dwells against its mechanical limit.
• Rail sized on stall. Data-sheet stall figure × rail voltage, then the next converter size up: 0.8 A × 6 V = 4.8 W points at a 10 W unit.
• One ground, one tie point. Logic and servo grounds meet once; every extra path is a noise loop.
• Bulk capacitance at the connector. 470-1000 µF bridges the stall transient while the converter responds.
• Pin map checked. The servo pin must be timer-capable; shared-pin conflicts show on the map, not by trial.
• Clamp, reject, deadband. 2° (44 mm) is the starting deadband; tighten only if the pointer must resolve finer.
• Rigid mounting. Sensor and servo share one rigid bracket; a moving sensor measures its own motion.
• Commissioning log. Range and angle printed together for the first hours — the pair tells you which side drifted.

When the surrounding station is yours to specify too, the system configurator walks the whole chain from camera to stand, and the contact page puts the odd cases in front of the applications team.