Robotic Arm Assembly

# labs/arm_math.py
SWEEP_DEG = 180.0        # shaft travel covered by one full command range


def joint_value(degrees: float) -> float:
    """0 deg -> -1.0, 90 deg -> 0.0, 180 deg -> +1.0, the same map chapter 64 built."""
    return degrees / (SWEEP_DEG / 2.0) - 1.0


def lerp(a: float, b: float, t: float) -> float:
    """The point fraction t of the way from a to b."""
    return a + (b - a) * t


if __name__ == "__main__":
    print(f"one command sweep is {SWEEP_DEG:.0f} deg: 0 -> {joint_value(0.0):+.2f}, "
          f"90 -> {joint_value(90.0):+.2f}, 180 -> {joint_value(180.0):+.2f}")
    bands = (("shoulder", 15.0, 165.0, 90.0), ("elbow", 0.0, 150.0, 30.0),
             ("wrist", 0.0, 180.0, 90.0), ("gripper", 0.0, 90.0, 0.0))
    for name, lo, hi, home in bands:
        print(f"{name:<9} band {lo:5.1f}-{hi:5.1f}  home {home:5.1f}"
              f"   value at band ends {joint_value(lo):+.2f} .. {joint_value(hi):+.2f}"
              f"   at home {joint_value(home):+.2f}")

$ uv run python -m labs.arm_math
one command sweep is 180 deg: 0 -> -1.00, 90 -> +0.00, 180 -> +1.00
shoulder  band  15.0-165.0  home  90.0   value at band ends -0.83 .. +0.83   at home +0.00
elbow     band   0.0-150.0  home  30.0   value at band ends -1.00 .. +0.67   at home -0.67
wrist     band   0.0-180.0  home  90.0   value at band ends -1.00 .. +1.00   at home +0.00
gripper   band   0.0- 90.0  home   0.0   value at band ends -1.00 .. +0.00   at home -1.00

One map serves all four joints, and that is the decision the rest of the chapter rests on. A servo's command range covers its entire sweep, near enough a hundred and eighty degrees of shaft, so degrees over ninety minus one is the whole conversion and it is chapter 64's arithmetic with a new name. What varies per joint is which slice of that map the joint may use, and nothing else. Read the two right-hand columns: the shoulder never emits anything outside plus or minus 0.83 because a printed bracket takes the rest, and the gripper spends its whole life in the bottom half, at -1.00 with the jaws open and 0.00 with them shut. Those are facts about printed parts, not about servos.

# labs/arm_math.py — added below joint_value
FRAME_US = 20000.0       # one 50 Hz frame
MIN_PULSE_US = 1000.0
MAX_PULSE_US = 2000.0
COUNTS = 4096            # the PCA9685 divides every frame into 12 bits


def value_to_pulse_us(value: float) -> float:
    """Undo the normalisation: a -1.0..+1.0 value becomes a duration."""
    value = max(-1.0, min(1.0, value))
    return MIN_PULSE_US + (value + 1.0) / 2.0 * (MAX_PULSE_US - MIN_PULSE_US)


def pulse_to_duty16(pulse_us: float) -> int:
    """The 16-bit number the driver library takes; the chip keeps the top 12 bits."""
    return int(pulse_us / FRAME_US * 65535)


if __name__ == "__main__":
    print(f"12 bits across a {FRAME_US / 1000:.0f} ms frame: one count is {FRAME_US / COUNTS:.2f} us")
    for value in (-1.00, -0.83, -0.67, 0.00, 0.67, 1.00):
        us = value_to_pulse_us(value)
        duty16 = pulse_to_duty16(us)
        count = duty16 >> 4
        print(f"value {value:+.2f} -> {us:7.1f} us -> duty16 {duty16:5d} -> count {count:4d}"
              f" -> {count * FRAME_US / COUNTS:7.1f} us on the wire")

$ uv run python -m labs.arm_math
12 bits across a 20 ms frame: one count is 4.88 us
value -1.00 ->  1000.0 us -> duty16  3276 -> count  204 ->   996.1 us on the wire
value -0.83 ->  1085.0 us -> duty16  3555 -> count  222 ->  1084.0 us on the wire
value -0.67 ->  1165.0 us -> duty16  3817 -> count  238 ->  1162.1 us on the wire
value +0.00 ->  1500.0 us -> duty16  4915 -> count  307 ->  1499.0 us on the wire
value +0.67 ->  1835.0 us -> duty16  6012 -> count  375 ->  1831.1 us on the wire
value +1.00 ->  2000.0 us -> duty16  6553 -> count  409 ->  1997.1 us on the wire

The library takes a sixteen-bit duty number so that one interface can cover boards of different resolutions, and this chip keeps the top twelve bits of it. Twelve bits over a twenty millisecond frame is 4.88 microseconds per count, so the board cannot hold every pulse you ask for. The worst case in that table is the bottom end: 1000 microseconds requested, 996.1 delivered, off by 3.9. Chapter 64 measured the deadband on this class of servo at around 10 microseconds of pulse, the window it treats as no error at all. The board's rounding lands inside that window, so the quantisation is not something the servo can act on, and there is no point carrying fractions of a count anywhere in the code.

# labs/arm_config.py
from dataclasses import dataclass


@dataclass(frozen=True)
class JointConfig:
    """One joint's output channel and the travel measured on the built arm."""
    channel: int
    min_deg: float
    max_deg: float
    home_deg: float


JOINTS: dict[str, JointConfig] = {
    "shoulder": JointConfig(channel=0, min_deg=15.0, max_deg=165.0, home_deg=90.0),
    "elbow":    JointConfig(channel=1, min_deg=0.0,  max_deg=150.0, home_deg=30.0),
    "wrist":    JointConfig(channel=2, min_deg=0.0,  max_deg=180.0, home_deg=90.0),
    "gripper":  JointConfig(channel=3, min_deg=0.0,  max_deg=90.0,  home_deg=0.0),
}


def clamp(cfg: JointConfig, degrees: float) -> float:
    """Pin a request into the band this joint physically has."""
    return max(cfg.min_deg, min(cfg.max_deg, degrees))


if __name__ == "__main__":
    print(JOINTS["gripper"])
    for name, asked in (("elbow", 90.0), ("elbow", 200.0), ("elbow", -20.0),
                        ("shoulder", 250.0), ("shoulder", 5.0), ("gripper", 120.0)):
        cfg = JOINTS[name]
        reached = clamp(cfg, asked)
        note = "" if reached == asked else "   clamped at this joint's own stop"
        print(f"{name:<9} asked {asked:+7.1f} -> {reached:6.1f} deg{note}")

$ uv run python -m labs.arm_config
JointConfig(channel=3, min_deg=0.0, max_deg=90.0, home_deg=0.0)
elbow     asked   +90.0 ->   90.0 deg
elbow     asked  +200.0 ->  150.0 deg   clamped at this joint's own stop
elbow     asked   -20.0 ->    0.0 deg   clamped at this joint's own stop
shoulder  asked  +250.0 ->  165.0 deg   clamped at this joint's own stop
shoulder  asked    +5.0 ->   15.0 deg   clamped at this joint's own stop
gripper   asked  +120.0 ->   90.0 deg   clamped at this joint's own stop

The dataclass earns its place three ways. It names the four numbers, so nobody has to remember whether the tuple went channel-min-max-home or channel-home-min-max. It prints itself when something looks wrong, which is the line at the top of that output. And it is frozen, because those numbers are measurements of a physical arm: a routine that quietly widens a limit at runtime has invented travel the printed parts do not have. Note also what the clamp catches at the bottom of the band. A shoulder asked for 5 degrees comes back at 15, and that guard is as necessary as the one at 250, since the bracket is in the way at both ends.

# labs/arm_driver.py
import board
import busio
from adafruit_pca9685 import PCA9685

from labs.arm_math import pulse_to_duty16, value_to_pulse_us


class ChannelDriver:
    """Owns the PCA9685 and speaks the same -1.0..+1.0 value the servos took before."""

    def __init__(self, frequency: int = 50) -> None:
        self._pca = PCA9685(busio.I2C(board.SCL, board.SDA))
        self._pca.frequency = frequency

    def write(self, channel: int, value: float) -> None:
        value = max(-1.0, min(1.0, value))
        self._pca.channels[channel].duty_cycle = pulse_to_duty16(value_to_pulse_us(value))

    def relax(self, channel: int) -> None:
        """No pulse at all, so the joint stops holding torque."""
        self._pca.channels[channel].duty_cycle = 0


class RecordingDriver:
    """Records writes instead of making them, so the logic runs on a laptop."""

    def __init__(self) -> None:
        self.writes = 0
        self.last: dict[int, float] = {}

    def write(self, channel: int, value: float) -> None:
        self.writes += 1
        self.last[channel] = max(-1.0, min(1.0, value))

    def relax(self, channel: int) -> None:
        self.last.pop(channel, None)

# labs/arm_control.py
import time

from labs.arm_config import JOINTS, JointConfig
from labs.arm_math import joint_value, lerp

STEPS = 40
STEP_DELAY = 0.015
HOME_ORDER = ("gripper", "wrist", "elbow", "shoulder")


class RoboticArm:
    def __init__(self, driver, configs: dict[str, JointConfig] = JOINTS) -> None:
        self._driver = driver
        self._configs = configs
        self._angles = {name: cfg.home_deg for name, cfg in configs.items()}

    def _set_joint(self, name: str, degrees: float) -> float:
        """The only place in the program that writes to a channel."""
        cfg = self._configs[name]
        deg = max(cfg.min_deg, min(cfg.max_deg, degrees))
        self._driver.write(cfg.channel, joint_value(deg))
        self._angles[name] = deg
        return deg

    def home(self) -> None:
        """One joint at a time, tip first, so the heavy one moves last and alone."""
        for name in HOME_ORDER:
            cfg = self._configs[name]
            deg = self._set_joint(name, cfg.home_deg)
            print(f"  {name:<8} ch{cfg.channel}  {deg:6.1f} deg  value {joint_value(deg):+.2f}")
            time.sleep(0.4)

    def pose(self) -> str:
        return "  ".join(f"{n}={self._angles[n]:6.1f}"
                         for n in ("shoulder", "elbow", "wrist", "gripper"))

$ uv run python -m labs.arm_dry_run
homing: gripper, wrist, elbow, shoulder
  gripper  ch3     0.0 deg  value -1.00
  wrist    ch2    90.0 deg  value +0.00
  elbow    ch1    30.0 deg  value -0.67
  shoulder ch0    90.0 deg  value +0.00
at home           shoulder=  90.0  elbow=  30.0  wrist=  90.0  gripper=   0.0

The driver arrives as a constructor argument, which is what lets that output come off a laptop with RecordingDriver and off the Pi with ChannelDriver without touching a line of the arm. More importantly it keeps the class ignorant of I2C: swap in a board with a different chip and nothing above write changes.

home is the one motion in the class that writes a target directly, and the reason is that nothing else can. A servo reports nothing back, so at power-up the code has no idea where the arm actually is, and there is no honest starting angle to interpolate from. Every other method leans on _angles, which is only true once homing has made it true. Moving one joint at a time with four tenths of a second between them is what keeps that first blind write survivable: the gripper and wrist fold in while they are still near the tip and cheap to swing, and the shoulder travels last, alone, with nothing else moving. On the first run of a newly built arm, keep a hand near the supply switch through that last step.

# labs/arm_control.py — continued inside RoboticArm

    def reach(self, shoulder_deg: float, elbow_deg: float,
              wrist_deg: float, steps: int = STEPS) -> None:
        """Walk all three arm joints to their targets together, in equal steps."""
        targets = {"shoulder": shoulder_deg, "elbow": elbow_deg, "wrist": wrist_deg}
        starts = {name: self._angles[name] for name in targets}
        for step in range(1, steps + 1):
            t = step / steps
            for name, target in targets.items():
                self._set_joint(name, lerp(starts[name], target, t))
            time.sleep(STEP_DELAY)

    def grab(self) -> None:
        self._set_joint("gripper", self._configs["gripper"].max_deg)

    def release(self) -> None:
        self._set_joint("gripper", self._configs["gripper"].home_deg)

    def relax(self) -> None:
        """Stop every pulse train so four servos stop holding against gravity."""
        for cfg in self._configs.values():
            self._driver.relax(cfg.channel)

# labs/arm_demo.py
import signal
import sys
import time

from labs.arm_control import RoboticArm
from labs.arm_driver import ChannelDriver


def main() -> None:
    arm = RoboticArm(ChannelDriver())

    def on_signal(signum: int, frame: object) -> None:
        arm.relax()
        sys.exit(0)

    signal.signal(signal.SIGINT, on_signal)
    signal.signal(signal.SIGTERM, on_signal)

    print("homing: " + ", ".join(("gripper", "wrist", "elbow", "shoulder")))
    arm.home()
    print(f"at home           {arm.pose()}")

    started = time.monotonic()
    arm.reach(45.0, 90.0, 90.0)
    print(f"reach(45, 90, 90) {arm.pose()}   {time.monotonic() - started:.2f} s")

    arm.grab()
    print(f"grab              {arm.pose()}")
    arm.reach(250.0, 90.0, 90.0)
    print(f"reach(250,90,90)  {arm.pose()}   shoulder clamped")
    arm.release()
    print(f"release           {arm.pose()}")
    arm.relax()


if __name__ == "__main__":
    main()

$ uv run python -m labs.arm_demo   # measured on the bench — yours will vary
homing: gripper, wrist, elbow, shoulder
  gripper  ch3     0.0 deg  value -1.00
  wrist    ch2    90.0 deg  value +0.00
  elbow    ch1    30.0 deg  value -0.67
  shoulder ch0    90.0 deg  value +0.00
at home           shoulder=  90.0  elbow=  30.0  wrist=  90.0  gripper=   0.0
reach(45, 90, 90) shoulder=  45.0  elbow=  90.0  wrist=  90.0  gripper=   0.0   0.64 s
grab              shoulder=  45.0  elbow=  90.0  wrist=  90.0  gripper=  90.0
reach(250,90,90)  shoulder= 165.0  elbow=  90.0  wrist=  90.0  gripper=  90.0   shoulder clamped
release           shoulder= 165.0  elbow=  90.0  wrist=  90.0  gripper=   0.0

The three joints are interleaved inside one step loop, not run one after another, and the difference is visible from across the room. Sequential moves draw a staircase through the workspace: the shoulder swings while the forearm is still folded, then the elbow opens into whatever is now in front of it, and a wrist that started low drags across the desk on the way. Stepping them together keeps the pose consistent all the way through, because every joint is the same fraction of its own journey at every step.

Nothing here eases. That is deliberate, and it is where the arm parts company with the eye. Smoothstep exists to make a gaze read as attention, and its peak rate is exactly 1.5 times its average, since the derivative of the curve tops out at t equal to a half. On two unloaded servos moving twenty five degrees that costs nothing. On three servos carrying an arm it means the middle of every motion draws half again the current of a flat profile, right when all three are moving fastest. A constant rate also gives you a number you can defend: this move is 45 degrees of shoulder over 40 steps, which is 1.125 degrees a step, and no step ever asks for more. Forty steps of 15 milliseconds is 0.60 seconds of sleeping, and the run above took 0.64, the extra coming from 120 I2C transactions on a bus clocked at 100 kHz.