Skip to content

armgit / moss

Public Imported from GitHub
0

MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.

Python

arm adapter: torque on only after every servo's goal register is latched to its present position (a servo enables onto the goal its register holds and would jump to a stale one); bench checks the write order

metrox2a5c0da1ed02
diff --git a/benches/bench_moss_arm_feetech_cold.py b/benches/bench_moss_arm_feetech_cold.py
index 9c4c897..39deaa7 100644
--- a/benches/bench_moss_arm_feetech_cold.py
+++ b/benches/bench_moss_arm_feetech_cold.py
@@ -9,8 +9,10 @@ Known input -> known output in physical units. Groups:
B. connect - the adapter opens the fake bus, reads the six servos, sets
speed 400 and acceleration 50 on the five arm servos, the
force 400 on the gripper's registers 44 and 48, writes no
- torque; activate turns torque on and holds the present
- position; limits = the calibration's end stops in radians
+ torque; activate latches every servo's goal register to its
+ present position (six writes of register 42, all before the
+ first torque write) then turns torque on; limits = the
+ calibration's end stops in radians
C. follow - a goal of +30 deg on the pan writes 2389 to register 42 of
servo 1 and the fake servo reaches it at 400 ticks/s: read
back +30.0 deg; the gripper to 53 mm writes 2780 and reads
@@ -186,8 +188,16 @@ check(ad.present_ticks() == {"shoulder_pan": 2857, "shoulder_lift": 860, "elbow_
q = ad.read_joint_positions()
check(abs(q[0] * DEG - 71.1) < 0.05 and abs(q[5] - 0.090) < 1e-9, f"positions: pan {q[0] * DEG:.1f} deg, gap {q[5] * 1000:.0f} mm")
check(not ad.read_enabled() and ad.write_joint_positions(q) is False, "no command before activate")
+n0 = len(bus.writes)
check(ad.activate() and ad.read_enabled() and all(s.torque == 1 for s in bus.servos.values()), "activate: torque on, six servos")
-check(ad.goal_ticks() == ad.present_ticks(), "torque on holds the present position")
+w = bus.writes[n0:]
+goals = [(sid, val) for sid, addr, val in w if addr == F.REG_GOAL]
+first_torque = next((i for i, (sid, addr, val) in enumerate(w) if addr == F.REG_TORQUE), None)
+last_goal = max((i for i, (sid, addr, val) in enumerate(w) if addr == F.REG_GOAL), default=None)
+check(len(goals) == 6 and all(val == bus.servos[sid].goal == int(round(bus.servos[sid].pos)) for sid, val in goals),
+ "before torque: every servo's goal register latched to its present position (six writes of register 42)")
+check(first_torque is not None and last_goal is not None and last_goal < first_torque, "all six goal writes come before the first torque write")
+check(ad.goal_ticks() == ad.present_ticks(), "the host's goal cache holds the present position too")
lim = ad.get_limits()
check(abs(lim.position_lower[1] * DEG + 104.4) < 0.05 and abs(lim.position_upper[0] * DEG - 118.3) < 0.05 and lim.position_upper[5] == 0.090,
f"limits: lift from {lim.position_lower[1] * DEG:.1f} deg, pan to {lim.position_upper[0] * DEG:.1f} deg, gap to 90 mm")
diff --git a/moss_dimos/arm_feetech.py b/moss_dimos/arm_feetech.py
index 90bfa93..62eac52 100644
--- a/moss_dimos/arm_feetech.py
+++ b/moss_dimos/arm_feetech.py
@@ -13,7 +13,9 @@ Per coordinator tick: one read of every servo's present position (register
56), then one goal write (register 42) for every servo whose target moved
by a tick or more. Goal speed (46) and acceleration (41) are set once at
connect: the servo's own ceiling, so a streamed position cannot ask more
-than the sweep's speed. Torque (40) goes on at activate and off at
+than the sweep's speed. Torque (40) goes on at activate, after every goal
+register has been set to the present position (a servo enables onto the
+goal its register holds, and would jump to a stale one), and off at
deactivate; the gripper's force limit goes to registers 44 and 48 at
connect.
@@ -401,15 +403,29 @@ class FeetechSo101Adapter:
return ok
def write_enable(self, enable: bool) -> bool:
+ """Torque on or off on the six servos. Before torque goes on, every
+ servo's goal register is set to its present position: a servo
+ enables onto whatever goal its register holds (the last run's,
+ after a power cycle anything), and the arm would jump there."""
if not self._connected:
return False
ok = True
+ if enable:
+ if self._read_all() is None:
+ self._error = (BUS_ERROR, "the servos do not answer before torque on")
+ return False
+ for j in self._joints():
+ if self._bus.write2(self._ids[j], REG_GOAL, self._present[j]):
+ self._goal[j] = self._present[j]
+ else:
+ ok = False
+ if not ok:
+ self._error = (BUS_ERROR, "a goal could not be latched before torque on; torque stays off")
+ return False
for j in self._joints():
ok &= self._bus.write1(self._ids[j], REG_TORQUE, 1 if enable else 0)
if ok:
self._enabled = bool(enable)
- if enable:
- self._goal = dict(self._present) # torque on: hold where the arm is
self._lag_since.clear()
return ok