Documentation

#include <Servo.h>

Servo base, shoulder, elbow;
int cur[3] = {1500, 1500, 1500};   // current pulse widths (us)

void setup() {
  base.attach(9);
  shoulder.attach(10);
  elbow.attach(11);
  writeAll();
  delay(1000);                      // settle at center
}

void writeAll() {
  base.writeMicroseconds(cur[0]);
  shoulder.writeMicroseconds(cur[1]);
  elbow.writeMicroseconds(cur[2]);
}

// glide all joints to a target pose together
void moveTo(int b, int s, int e, int stepDelay) {
  int tgt[3] = {b, s, e};
  bool moving = true;
  while (moving) {
    moving = false;
    for (int i = 0; i < 3; i++) {
      if (cur[i] < tgt[i]) { cur[i] += 5; moving = true; }
      if (cur[i] > tgt[i]) { cur[i] -= 5; moving = true; }
    }
    writeAll();
    delay(stepDelay);
  }
}

void loop() {
  moveTo(1200, 1700, 1300, 6);   // reach forward-left
  delay(600);
  moveTo(1800, 1400, 1700, 6);   // swing right, tuck
  delay(600);
  moveTo(1500, 1500, 1500, 6);   // home
  delay(1000);
}

// Library Manager: install "ESP32Servo"
#include <ESP32Servo.h>

Servo joints[3];
const int PINS[3] = {25, 26, 27};

void setup() {
  for (int i = 0; i < 3; i++) {
    joints[i].setPeriodHertz(50);
    joints[i].attach(PINS[i], 500, 2500);
    joints[i].write(90);            // center everything
  }
  delay(1000);
}

void glide(int idx, int from, int to) {
  int dir = (to > from) ? 1 : -1;
  for (int a = from; a != to; a += dir) {
    joints[idx].write(a);
    delay(8);
  }
}

void loop() {
  glide(0, 90, 40);    // base left
  glide(1, 90, 120);   // shoulder down
  glide(2, 90, 60);    // elbow in
  delay(500);
  glide(2, 60, 90);
  glide(1, 120, 90);
  glide(0, 40, 90);
  delay(1200);
}

from gpiozero import AngularServo
from gpiozero.pins.pigpio import PiGPIOFactory
from time import sleep

# sudo apt install pigpio; sudo systemctl start pigpiod
# pigpio gives hardware-timed pulses = no servo jitter
factory = PiGPIOFactory()

def make(pin):
    return AngularServo(pin, min_angle=0, max_angle=180,
                        min_pulse_width=0.0005, max_pulse_width=0.0025,
                        pin_factory=factory)

base, shoulder, elbow = make(17), make(27), make(22)

def glide(servo, start, end, step=2, dt=0.02):
    rng = range(start, end + 1, step) if end > start else range(start, end - 1, -step)
    for a in rng:
        servo.angle = a
        sleep(dt)

# center, then run a small routine
for s in (base, shoulder, elbow):
    s.angle = 90
sleep(1)

while True:
    glide(base, 90, 45)
    glide(shoulder, 90, 125)
    glide(elbow, 90, 60)
    sleep(0.5)
    glide(elbow, 60, 90)
    glide(shoulder, 125, 90)
    glide(base, 45, 90)
    sleep(1)

from machine import Pin, PWM
import time

PINS = (13, 14, 15)      # base, shoulder, elbow
servos = []
for p in PINS:
    pwm = PWM(Pin(p))
    pwm.freq(50)
    servos.append(pwm)

def write_us(pwm, us):
    pwm.duty_u16(int(us * 65535 / 20000))

pose = [1500, 1500, 1500]
for s, us in zip(servos, pose):
    write_us(s, us)
time.sleep(1)

def move_to(target, step=6, dt=0.01):
    while pose != list(target):
        for i in range(3):
            if pose[i] < target[i]: pose[i] = min(pose[i] + step, target[i])
            elif pose[i] > target[i]: pose[i] = max(pose[i] - step, target[i])
            write_us(servos[i], pose[i])
        time.sleep(dt)

while True:
    move_to((1250, 1750, 1300))
    time.sleep(0.5)
    move_to((1750, 1350, 1700))
    time.sleep(0.5)
    move_to((1500, 1500, 1500))
    time.sleep(1)