"""arm_body.py: a small servo arm on the GrowBot's own servo sockets. OPTIONAL. 1026. NOTHING INSTALLS THIS FILE. The /build USB installer never writes it, and a robot without it runs the stock firmware unchanged: relay_chip.py tries `import arm_body`, gets ImportError, and never drives a socket past the legs or wheels. A builder who adds an arm copies arm_body.py and arm_config.py onto the Pico by hand (Thonny or mpremote, after the USB install), then sets the sockets and limits in arm_config.py. Needs the Waveshare Pico Servo Driver (or a bare Pico) with the legs or wheels on GP0/GP1: socket N = GPIO N, driven by the Pico's own 50 Hz PWM. Refused on any other board: a PCA9685 carrier (its sockets are not Pico pins), and a direct-wire board with another pin map, like the Kitronik Simply Robotics, whose GP2-9 are its motor driver inputs (1026 review). WIRE (phone -> chip over the relay, forwarded verbatim; relay_chip.py hands every {"t":"arm"} frame here): {"t":"arm","a":"d1,d2,d3,d4"} absolute degrees, one per joint in arm_config order. A blank field leaves that joint alone ("90,,45" moves joints 1 and 3). Every value is clamped to its joint's min/max; a field that is not a number refuses the whole frame. {"t":"arm","pose":"wave"} a named pose: rest, up, point, wave, grab-open, grab-close {"t":"arm","stop":1} the arm goes limp (the legs are not touched) {"t":"arm","q":1} what is this arm: limits, rest, held joints, pose names. With no rid it is the arm page's keepalive: no reply, it only renews the lease (below) {"t":"stop"} the whole robot stops: legs AND arm go limp (relay_chip.py) A frame with a "rid" gets an ack, like the act lane. One without is a latest-wins stream (a slider drag). Use string rids ("arm7"): the app keys its gesture acks by numbered rid, so these can never collide. chip -> phone: {"t":"ack","rid":r,"ok":1,"queued_ms":ms,"arm":"90,90,90,90"} ms until the move or pose ends; arm = aim {"t":"ack","rid":r,"ok":0,"queued_ms":0,"arm":"...","error":"bad_angles"|"unknown_pose"|"nothing_to_do"} q adds "n", "names", "min", "max", "rest", "held" ("1,1,0,0"), "dps", "idle_s", "lease_s", "hold_s", "poses". MOTION * slew limit: a powered joint never moves faster than SLEW_DPS (default 60 deg/s). It steps once per main-loop tick (~50 Hz), and a stalled tick (a slow socket read) never turns into a jump: one tick moves at most MAX_STEP_MS worth of slew. * holds: after a move the arm keeps its pose (the legs go limp 300 ms after a gesture; an arm would drop). It goes limp after IDLE_S (default 120 s) holding still with no new command, on STOP, whenever the relay link drops, and when no {"t":"arm"} frame at all has come for LEASE_S (default 15 s). The relay never tells the chip that a phone left, so the lease is how a phone that slept, lost signal or closed the page lets go of the arm: the arm page sends a keepalive every 5 s while it is open and in sight. * hold_s (optional, per joint): that joint alone lets go sooner than IDLE_S. A grip closed on something is a stalled servo drawing its stall current the whole time it holds, so the template gives the grip 15 s. * waking: a limp joint's position is remembered as a best guess, and the next command wakes it there. A servo cannot say where it really is, so a joint that sagged or was pushed while limp snaps back to that guess at the servo's own speed, and only then slews. Limp joints wake one at a time, WAKE_MS apart, so that jump is never four servos' start current at once (a brownout is what used to kill Wi-Fi). * power-on: parks at rest the same way, one joint at a time, then goes limp. An arm left at rest barely moves. * nothing here loops or sleeps, except park(), which sleeps through the firmware's watchdog-fed sleep. tick(), handle() and limp() never raise: an arm fault limps the arm and never drops the relay link. """ FREQ = 50 _MIN_US, _MAX_US, _PERIOD_US = 500, 2500, 20000 # the same pulse map as PicoRobotics.py direct-wire MAX_STEP_MS = 100 # one tick never moves more than this much time's worth of slew MAX_JOINTS = 8 WAKE_MS = 400 # limp joints come back on one at a time, this far apart (the power-on park too) LEASE_MS = 15000 # default lease: no {"t":"arm"} frame for this long and a holding arm lets go WAVESHARE = {1: 0, 3: 1} # PicoRobotics.GPIO_PINS on the one board whose spare sockets are all servo sockets # Never an arm socket, whatever arm_config.py says: 0/1 are the legs or wheels, 8/9 are the I2C pins the motor # driver probes for a servo chip at boot. PWM outputs repeat every 16 pins on the RP2040/RP2350 (GP16 drives # GP0's channel), so every check is modulo 16. 23/24/25/29 belong to the Wi-Fi chip on a Pico W. NEVER = (0, 1, 8, 9) RADIO = (23, 24, 25, 29) # Named poses: keyframes of (offsets from each joint's neutral in arm_config order, None = leave that joint; # ms to hold once the keyframe is reached). UNMEASURED: written for base, shoulder, elbow, grip with neutral = # arm straight up, gripper half open. A builder retunes them with POSES in arm_config.py. "rest" is not here: # it is every joint's own rest from arm_config.py. DEFAULT_POSES = { "up": [([0, 0, 0, None], 0)], "point": [([0, 40, -30, None], 0)], "wave": [([0, 0, 0, None], 150), ([None, None, 30, None], 0), ([None, None, -30, None], 0), ([None, None, 30, None], 0), ([None, None, -30, None], 0), ([None, None, 0, None], 0)], "grab-open": [([None, None, None, 40], 0)], "grab-close": [([None, None, None, -40], 0)], } def duty(deg): deg = 0 if deg < 0 else 180 if deg > 180 else deg return int((_MIN_US + (_MAX_US - _MIN_US) * deg / 180) / _PERIOD_US * 65535) def _num(v): # a finite number as a float, else None (bools, strings, NaN and inf are not numbers here) if isinstance(v, bool) or not isinstance(v, (int, float)): return None return float(v) if -1e6 < v < 1e6 else None def _clip(v, lo, hi): return lo if v < lo else hi if v > hi else v def check_joints(joints, never=NEVER): """arm_config JOINTS -> (clean list, None) or (None, why). Refuses anything that could drive a pin the robot needs: the legs or wheels, the I2C probe pins, the radio pins, or two joints on one PWM output.""" if not isinstance(joints, (list, tuple)) or not 1 <= len(joints) <= MAX_JOINTS: return None, "JOINTS must list 1 to %d joints" % MAX_JOINTS taken = [g % 16 for g in never] out = [] for i, j in enumerate(joints): n = i + 1 if not isinstance(j, dict): return None, "joint %d is not a {...}" % n g = j.get("socket") if isinstance(g, bool) or not isinstance(g, int) or not 0 <= g <= 28 or g in RADIO: return None, "joint %d: socket must be a pin number, like 2 to 7 or 10 to 15" % n if g % 16 in taken: return None, "joint %d: socket %d is taken (0/1 legs or wheels, 8/9 I2C, or another joint)" % (n, g) lo, hi = _num(j.get("min")), _num(j.get("max")) if lo is None or hi is None or not 0 <= lo < hi <= 180: return None, "joint %d: needs min < max, both 0 to 180" % n mid = _num(j.get("neutral", (lo + hi) / 2)) rest = _num(j.get("rest", mid)) if mid is not None else None if mid is None or rest is None or not lo <= mid <= hi or not lo <= rest <= hi: return None, "joint %d: neutral and rest must sit inside min..max" % n dps = j.get("dps") if dps is not None: dps = _num(dps) if dps is None or not 1 <= dps <= 360: return None, "joint %d: dps must be 1 to 360" % n hold = j.get("hold_s") if hold is not None: hold = _num(hold) if hold is None or not 1 <= hold <= 3600: return None, "joint %d: hold_s must be 1 to 3600 seconds" % n hold = int(hold * 1000) taken.append(g % 16) out.append({"name": str(j.get("name", "j%d" % n)), "socket": g, "min": lo, "max": hi, "neutral": mid, "rest": rest, "dps": dps, "hold_ms": hold}) return out, None def clean_poses(poses): """POSES -> {name: [(offsets, hold_ms)]}, dropping any pose that is not that shape. None = the defaults.""" if poses is None: poses = DEFAULT_POSES out = {} if not isinstance(poses, dict): return out for name, frames in poses.items(): if not isinstance(name, str) or name == "rest" or not isinstance(frames, (list, tuple)) or not 1 <= len(frames) <= 16: continue good = [] for f in frames: try: offs, hold = f hold = _num(hold) if hold is None or not 0 <= hold <= 10000 or not isinstance(offs, (list, tuple)) or len(offs) > MAX_JOINTS: break vals = [None if o is None else _num(o) for o in offs] if any(v is None and o is not None for v, o in zip(vals, offs)): break good.append((vals, int(hold))) except (TypeError, ValueError): break if len(good) == len(frames): out[name] = good return out class Arm: def __init__(self, pwms, joints, ticks_ms, ticks_diff, slew_dps=60, idle_ms=120000, poses=None, lease_ms=LEASE_MS): self.pwms = pwms # one PWM-like object per joint: duty_u16(int) self.j = joints # check_joints() output self.n = len(joints) self.ticks_ms = ticks_ms self.ticks_diff = ticks_diff slew = _clip(_num(slew_dps) or 60, 1, 360) self.dps = [jt["dps"] or slew for jt in joints] self.hold_ms = [jt.get("hold_ms") for jt in joints] self.idle_ms = idle_ms self.lease_ms = lease_ms self.poses = clean_poses(poses) self.pos = [None] * self.n # best guess of where each joint is (None = never placed) self.target = [jt["rest"] for jt in joints] self.on = [False] * self.n # powered (pulsing) joints self.waiting = [] # limp joints given a target, in the order they will wake self.woke_t = None # when the last joint woke (None = the next may wake at once) self._duty = [None] * self.n # last duty written (None = unknown, write next time) self.seq = [] # keyframes still to play after the current one self.hold = 0 # ms to hold once the current keyframe is reached self.reached_t = None # when every powered joint reached its target self.t_last = ticks_ms() self.heard_t = self.t_last # the last {"t":"arm"} frame (the lease) self.why = None # why it last let go: "stop", "idle", "lease", "hold", "fault" # ---------- outputs ---------- def _out(self, i, deg): d = duty(deg) if d != self._duty[i]: self.pwms[i].duty_u16(d) self._duty[i] = d def _aim(self, targets): # point joints at targets (None = leave that joint as it is, powered or limp). A limp joint joins the wake # queue at where it was last seen (rest if never placed); _wake() turns it on and it slews from there. for i in range(self.n): t = targets[i] if i < len(targets) else None if t is None: continue jt = self.j[i] self.target[i] = _clip(t, jt["min"], jt["max"]) if not self.on[i] and i not in self.waiting: if self.pos[i] is None: self.pos[i] = jt["rest"] self.waiting.append(i) self.reached_t = None self._wake(self.ticks_ms()) # 1026 review: t_last is tick()'s own (it was reset here, so a slider stream at 25 Hz slewed at about half speed) def _wake(self, now): """The next waiting joint back on, at most one per WAKE_MS. Its first pulse is its last known position: a joint that sagged while limp snaps back there at the servo's own speed (no servo can report where it is), so they come back one at a time, never four start currents at once. Returns the joint, or None.""" if not self.waiting or (self.woke_t is not None and self.ticks_diff(now, self.woke_t) < WAKE_MS): return None i = self.waiting.pop(0) self.on[i] = True self._duty[i] = None self.woke_t = now self._out(i, self.pos[i]) return i def _let_go(self, i): self.on[i] = False if self._duty[i] != 0: try: self.pwms[i].duty_u16(0) self._duty[i] = 0 except Exception: self._duty[i] = None def limp(self, why="stop"): """Every joint limp, any pose dropped. Never raises: the main loop calls it on every re-dial.""" self.seq = [] self.waiting = [] self.woke_t = None self.reached_t = None for i in range(self.n): if self.on[i]: self.why = why self._let_go(i) return True # ---------- the main loop ---------- def tick(self): """Call every main-loop pass. A few float ops and at most n PWM writes; never blocks, never raises. Returns True while any joint is powered.""" try: return self._tick() except Exception as e: print("arm fault:", e) self.limp("fault") return False def _tick(self): now = self.ticks_ms() dt = self.ticks_diff(now, self.t_last) self.t_last = now if self.woke_t is not None and self.ticks_diff(now, self.woke_t) >= WAKE_MS: self.woke_t = None # gap over (and a stamp days old can never wrap round and block) if not self.waiting and not any(self.on): return False if self.ticks_diff(now, self.heard_t) >= self.lease_ms: self.limp("lease") # no arm frame for LEASE_S: the phone slept, lost signal or left return False dt = _clip(dt, 0, MAX_STEP_MS) woke = self._wake(now) moving = bool(self.waiting) for i in range(self.n): if not self.on[i]: continue p, t = self.pos[i], self.target[i] if p != t: if i != woke: # just woken: this tick's pulse is where it was; slew from the next step = self.dps[i] * dt / 1000.0 p = t if abs(t - p) <= step else (p + step if t > p else p - step) self.pos[i] = p self._out(i, p) if p != t: moving = True if moving: return True if self.reached_t is None: self.reached_t = now still = self.ticks_diff(now, self.reached_t) if self.seq: if still >= self.hold: targets, self.hold = self.seq.pop(0) self._aim(targets) return True if still >= self.idle_ms: self.limp("idle") return False for i in range(self.n): h = self.hold_ms[i] if self.on[i] and h is not None and still >= h: self._let_go(i) # hold_s: a grip squeezing something is a stalled servo self.why = "hold" return any(self.on) # ---------- messages ---------- def handle(self, m): """One {"t":"arm"} frame -> the ack to send (a dict), or None when the frame has no rid. Never raises. Every frame, even one refused, renews the lease: someone is still there.""" try: self.heard_t = self.ticks_ms() err = None if m.get("stop"): self.limp("stop") elif "pose" in m: err = self.start_pose(m.get("pose")) elif "a" in m: err = self.move(m.get("a")) elif not m.get("q"): err = "nothing_to_do" if "rid" not in m: return None r = {"t": "ack", "rid": m.get("rid"), "ok": 0 if err else 1, "queued_ms": 0 if err else self.queued_ms(), "arm": self.where()} if err: r["error"] = err if m.get("q"): r.update(self.info()) return r except Exception as e: print("arm fault:", e) self.limp("fault") return {"t": "ack", "rid": m.get("rid"), "ok": 0, "queued_ms": 0, "error": "arm_fault"} if "rid" in m else None def move(self, a): if not isinstance(a, str): return "bad_angles" parts = a.split(",") if len(parts) > self.n: return "bad_angles" targets = [] for p in parts: p = p.strip() if not p: targets.append(None) continue try: v = float(p) except ValueError: return "bad_angles" if not -1e6 < v < 1e6: # NaN fails this too return "bad_angles" targets.append(v) if all(t is None for t in targets): return "nothing_to_do" self.seq = [] self.hold = 0 self._aim(targets) return None def start_pose(self, name): if name == "rest": frames = [([jt["rest"] for jt in self.j], 0)] else: kf = self.poses.get(name) if isinstance(name, str) else None if not kf: return "unknown_pose" frames = [([None if k >= len(offs) or offs[k] is None else self.j[k]["neutral"] + offs[k] for k in range(self.n)], hold) for offs, hold in kf] first, self.hold = frames[0] self.seq = frames[1:] self._aim(first) return None # ---------- reporting ---------- def _move_ms(self, frm, to): return max([abs(to[i] - frm[i]) * 1000.0 / self.dps[i] for i in range(self.n)] or [0]) def _aimed(self, i): return self.on[i] or i in self.waiting def queued_ms(self): """Estimated ms until the current move or pose ends (the ticks land within one tick per keyframe and per wake). A waiting joint first waits for its turn to wake, WAKE_MS after the one before it.""" p = [self.pos[i] if self.pos[i] is not None else self.j[i]["rest"] for i in range(self.n)] cur = [self.target[i] if self._aimed(i) else p[i] for i in range(self.n)] gap = 0 if self.woke_t is None else _clip(WAKE_MS - self.ticks_diff(self.ticks_ms(), self.woke_t), 0, WAKE_MS) total = 0 for i in range(self.n): wait = gap + self.waiting.index(i) * WAKE_MS if i in self.waiting else 0 total = max(total, wait + abs(cur[i] - p[i]) * 1000.0 / self.dps[i]) p, hold = cur, self.hold for targets, h in self.seq: nxt = [p[i] if i >= len(targets) or targets[i] is None else _clip(targets[i], self.j[i]["min"], self.j[i]["max"]) for i in range(self.n)] total += hold + self._move_ms(p, nxt) p, hold = nxt, h return int(total + 0.5) def where(self): vals = [self.target[i] if self._aimed(i) else (self.pos[i] if self.pos[i] is not None else self.j[i]["rest"]) for i in range(self.n)] return ",".join(str(int(v + 0.5)) for v in vals) def info(self): def row(key): return ",".join(str(int(jt[key] + 0.5)) for jt in self.j) return {"n": self.n, "names": ",".join(jt["name"] for jt in self.j), "min": row("min"), "max": row("max"), "rest": row("rest"), "held": ",".join("1" if o else "0" for o in self.on), "dps": ",".join(str(int(d + 0.5)) for d in self.dps), "idle_s": self.idle_ms // 1000, "lease_s": self.lease_ms // 1000, "hold_s": ",".join("0" if h is None else str(int(h / 1000 + 0.5)) for h in self.hold_ms), "poses": ["rest"] + sorted(self.poses)} # ---------- power-on ---------- def park(self, sleep_ms, stagger_ms=WAKE_MS, settle_ms=600): """Power-on: each joint to rest, one at a time, then limp. sleep_ms must feed the watchdog (relay_chip.py passes sleep_fed). 4 joints = 2.2 s.""" try: for i in range(self.n): r = self.j[i]["rest"] self.pos[i] = self.target[i] = r self.on[i] = True self._duty[i] = None self._out(i, r) sleep_ms(stagger_ms) sleep_ms(settle_ms) except Exception as e: print("arm fault:", e) self.limp("park") def for_firmware(board, ticks_ms, ticks_diff): """relay_chip.py's one call: an Arm, or None (and a printed reason) when there is no usable arm_config.py.""" try: import arm_config as cfg except ImportError: print("arm: no arm_config.py next to arm_body.py; the arm stays off") return None if board is None or not hasattr(board, "pwm"): print("arm: needs the direct-wire servo board (Waveshare Pico Servo Driver); the arm stays off") return None try: import PicoRobotics legs = dict(PicoRobotics.GPIO_PINS) except Exception: legs = None if legs != WAVESHARE: # 1026 review: another direct-wire map (Kitronik Simply Robotics {1: 15, 3: 13}) has its motor driver # inputs on GP2-9, so "socket N = GPIO N" would drive motor pins. Only the Waveshare map is known safe. print("arm: sockets 2 to 15 are servo sockets only on the Waveshare Pico Servo Driver, with the legs or " "wheels on GP0 and GP1 (PicoRobotics.GPIO_PINS %r); the arm stays off" % (legs,)) return None joints, why = check_joints(getattr(cfg, "JOINTS", None)) if why: print("arm: arm_config.py " + why + "; the arm stays off") return None slew = _clip(_num(getattr(cfg, "SLEW_DPS", 60)) or 60, 1, 360) idle_s = _clip(_num(getattr(cfg, "IDLE_S", 120)) or 120, 1, 3600) lease_s = _clip(_num(getattr(cfg, "LEASE_S", LEASE_MS // 1000)) or LEASE_MS // 1000, 5, 600) from machine import Pin, PWM pwms = [] for jt in joints: p = PWM(Pin(jt["socket"])) p.freq(FREQ) p.duty_u16(0) pwms.append(p) arm = Arm(pwms, joints, ticks_ms, ticks_diff, slew, int(idle_s * 1000), getattr(cfg, "POSES", None), int(lease_s * 1000)) print("arm: %d joints on sockets %s, %d deg/s, limp after %d s still or %d s with no arm page" % (arm.n, ",".join(str(jt["socket"]) for jt in joints), int(slew), int(idle_s), int(lease_s))) return arm