Public: anyone can read this; the crew can change it.

Migrated from github.com/Trevato/SO-ARM100

workbenchthis machine

Precision, forever: calibrate the model to the arm, tune the servos against the scorecard, report weekly #507

Parked opened this in answer to
Closed without merging

smith/arm:smith/439-precision-forever-calibrate-the-model-to → main 10 commits fork

  • Closed without merging. Nothing was applied to main.

Review

Sign in to review this change.
9 files changed +368 −9
Software/README.md +14
@@ -1024,6 +1024,20 @@ anyone else gets a 404 through the relay.
10241024 that held torque gets it back where it rests.
10251025 - `POST /servo-params/factory {"id": n}` writes back the values recorded at
10261026 first read.
1027 - The arm server owns the gains. At start, and when a servo that stopped
1028 answering answers again (a swap), it writes `settings.servoGains` to each
1029 servo that lacks them, only while nobody drives. The defaults are what the
1030 steward measured on the real arm, 2026-10-21: the shipped P16 I0 settled 24-27
1031 ticks (about 2.4°) short and never arrived; P64 arrives in 150-330 ms with at
1032 most 3 ticks of overshoot (shoulder and elbow also I4). The gripper stays at
1033 the factory P16, torque-capped on purpose. The factory file is written
1034 before the first gain, so `POST /servo-params/factory` still restores P16;
1035 the relay's `servo-factory.json` is read after the start-up write and may
1036 already show P64.
1037 - The page keeps a released slider's intent for 1 s (three times the slowest
1038 settle), arrival within 12 ticks. `bun bench/real-probe.ts <arm server>
1039 --joint 2 [--step 60]` times one small step on the real arm, refuses while a
1040 turn is held, and always moves the joint back to where it started.
10271041 - Both writes are refused, before the bus is read, with 409 while a stream,
10281042 move, playback or recording drives and 423 during an E-stop or hold. Each
10291043 appears in the machine log.
Software/so101/bench/real-probe.test.ts added +26
@@ -0,0 +1,26 @@
1 import { expect, test } from "bun:test";
2 import { plan, summarize } from "./real-probe.ts";
3
4 const run = (positions: number[]) => positions.map((position, i) => ({ t: i * 50, position }));
5
6 test("arrival is the start of the last stay within the tolerance, overshoot is past the target", () => {
7 const r = summarize(run([2048, 2080, 2112, 2125, 2130, 2128]), 2048, 2120);
8 expect(r).toEqual({ arriveMs: 100, finalError: -8, overshoot: 10 });
9 });
10
11 test("a joint that settles short never arrives", () => {
12 const r = summarize(run([2048, 2060, 2070, 2080, 2084]), 2048, 2108);
13 expect(r).toEqual({ arriveMs: null, finalError: 24, overshoot: 0 });
14 });
15
16 test("a downward step reads the same way", () => {
17 expect(summarize(run([2048, 2000, 1990]), 2048, 1988)).toEqual({ arriveMs: 50, finalError: 2, overshoot: 0 });
18 });
19
20 test("a step is small and stays inside the range, turning round when there is no room", () => {
21 expect(plan(2000, 60, 800, 3200)).toEqual({ to: 2060 });
22 expect(plan(3180, 60, 800, 3200)).toEqual({ to: 3120 });
23 expect(() => plan(2000, 0, 800, 3200)).toThrow();
24 expect(() => plan(2000, 151, 800, 3200)).toThrow();
25 expect(() => plan(2000, 60, 1990, 2010)).toThrow("no room");
26 });
Software/so101/bench/real-probe.ts added +143
@@ -0,0 +1,143 @@
1 /**
2 * One joint of the real arm, one small step, timed: how long it takes to arrive, where it settles, how far it overshoots.
3 * bun bench/real-probe.ts <http://arm-server:8101> --joint 2 [--step 60] [--settle 1500]
4 * It refuses while anyone drives, a turn is held, or the arm is stopped. It puts the joint back where it started
5 * after every run, however the run ended: repeated one-way steps walk a gravity joint into its limit (the elbow walked
6 * 300 ticks to its clamp), so the start pose is restored and checked, and a run that cannot restore it says so loudly.
7 */
8
9 /** Arrival tolerance in ticks: the page's intent hold uses the same (src/ui/intent.js). */
10 export const NEAR = 12;
11 /** The largest step a probe may take, ticks (about 13 degrees). */
12 export const MAX_STEP = 150;
13
14 export interface Sample {
15 /** Milliseconds from the move's start. */
16 t: number;
17 position: number;
18 }
19
20 export interface Result {
21 /** First time within NEAR of the target and staying there, ms; null when it never did. */
22 arriveMs: number | null;
23 /** Ticks short of (or past) the target at the last sample, signed in the direction of travel: positive is short. */
24 finalError: number;
25 /** Most ticks past the target in the direction of travel; 0 when none. */
26 overshoot: number;
27 }
28
29 /** The numbers of one step from `from` to `to`. Pure: the samples are the only input. */
30 export function summarize(samples: readonly Sample[], from: number, to: number, near = NEAR): Result {
31 const dir = Math.sign(to - from) || 1;
32 const short = (p: number) => (to - p) * dir;
33 let arriveMs: number | null = null;
34 let overshoot = 0;
35 for (const s of samples) {
36 if (Math.abs(to - s.position) <= near) arriveMs ??= s.t;
37 else arriveMs = null; // it left again: arrival is the start of the last stay
38 overshoot = Math.max(overshoot, -short(s.position));
39 }
40 const last = samples.at(-1);
41 return { arriveMs, finalError: last ? short(last.position) : Math.abs(to - from), overshoot };
42 }
43
44 /** The start and target for a step, kept inside the joint's calibrated range; a step that has no room either way is refused. */
45 export function plan(start: number, step: number, min: number, max: number): { to: number } {
46 if (!Number.isInteger(step) || step === 0 || Math.abs(step) > MAX_STEP) throw new Error(`step must be a whole number of ticks, not 0, at most ${MAX_STEP} either way`);
47 const to = start + step;
48 if (to >= min && to <= max) return { to };
49 const back = start - step;
50 if (back >= min && back <= max) return { to: back };
51 throw new Error(`no room for a ${Math.abs(step)}-tick step at ${start} (range ${min}-${max})`);
52 }
53
54 interface Joint {
55 id: number;
56 position: number | null;
57 lo: number | null;
58 hi: number | null;
59 }
60 interface Status {
61 joints: Joint[];
62 busy: boolean;
63 driving: string | null;
64 estop: unknown;
65 held: unknown;
66 }
67
68 const sleep = (ms: number) => new Promise((r) => setTimeout(r, ms));
69
70 /** Run one probe against the arm server at `base`; the joint is back at its start when this returns or throws (unless the arm stopped it). */
71 export async function probe(base: string, id: number, step: number, settleMs: number, now = () => performance.now()): Promise<Result & { id: number; start: number; to: number; returned: boolean }> {
72 const get = async (): Promise<Status> => (await fetch(`${base}/status`)).json() as Promise<Status>;
73 const move = async (to: number) => {
74 const res = await fetch(`${base}/move`, { method: "POST", body: JSON.stringify({ targets: { [id]: to } }) });
75 if (!res.ok) throw new Error(`move refused (${res.status}): ${((await res.json().catch(() => ({}))) as { error?: string }).error ?? ""}`);
76 };
77 const first = await get();
78 if (first.estop) throw new Error("refused: the arm is stopped");
79 if (first.held) throw new Error("refused: the arm is held; resume first");
80 if (first.busy) throw new Error(`refused: ${first.driving ?? "something"} is driving the arm`);
81 const joint = first.joints.find((j) => j.id === id);
82 if (!joint || joint.position === null || joint.lo === null || joint.hi === null) throw new Error(`servo ${id} is not answering`);
83 const start = joint.position;
84 const { to } = plan(start, step, joint.lo, joint.hi);
85
86 const samples: Sample[] = [];
87 const t0 = now();
88 let sampling = true;
89 const sampler = (async () => {
90 while (sampling) {
91 const p = (await get().catch(() => null))?.joints.find((j) => j.id === id)?.position;
92 if (typeof p === "number") samples.push({ t: Math.round(now() - t0), position: p });
93 await sleep(20);
94 }
95 })();
96 let returned = false;
97 try {
98 await move(to);
99 await sleep(settleMs);
100 } finally {
101 sampling = false;
102 await sampler;
103 // Always back to the start, then check it got there; a failure here is the one to read first.
104 try {
105 await move(start);
106 await sleep(settleMs);
107 const back = (await get()).joints.find((j) => j.id === id)?.position;
108 returned = typeof back === "number" && Math.abs(back - start) <= NEAR;
109 } catch (err) {
110 console.error(`COULD NOT RETURN servo ${id} to ${start}: ${err instanceof Error ? err.message : err}`);
111 }
112 }
113 return { id, start, to, returned, ...summarize(samples, start, to) };
114 }
115
116 if (import.meta.main) {
117 const [base, ...rest] = process.argv.slice(2);
118 const o = { joint: 0, step: 60, settle: 1500 };
119 try {
120 if (!base || !/^https?:\/\/[^/\s]+$/.test(base)) throw new Error("first argument is the arm server, http://host:port");
121 while (rest.length) {
122 const k = rest.shift();
123 const v = rest.shift() ?? "";
124 if (!/^-?\d{1,5}$/.test(v)) throw new Error(`bad value for ${k}`);
125 if (k === "--joint") o.joint = Number(v);
126 else if (k === "--step") o.step = Number(v);
127 else if (k === "--settle") o.settle = Number(v);
128 else throw new Error(`bad argument ${k}`);
129 }
130 if (o.joint < 1 || o.joint > 6 || o.settle < 300 || o.settle > 10000) throw new Error("--joint is 1-6, --settle 300-10000 ms");
131 } catch (err) {
132 console.error(`${err instanceof Error ? err.message : err}\nusage: bun bench/real-probe.ts <http://arm-server:8101> --joint 1-6 [--step 60] [--settle 1500]`);
133 process.exit(2);
134 }
135 try {
136 const r = await probe(base, o.joint, o.step, o.settle);
137 console.log(JSON.stringify(r));
138 if (!r.returned) process.exit(1);
139 } catch (err) {
140 console.error(err instanceof Error ? err.message : err);
141 process.exit(1);
142 }
143 }
Software/so101/src/machine-feed.test.ts +1 −1
@@ -107,7 +107,7 @@ test("a clamp shows up in the log the moment it is applied, with the command tha
107107 expect(goal.seq).toBeGreaterThan(clamp.seq);
108108 expect(p.log.some((e) => e.what === "Goal_Position" && ((e.detail.values as Record<string, number>)["1"] ?? 0) > 3457)).toBe(false);
109109 await p.until(() => p.messages.find((m) => m.type === "status" && Number.parseInt(String(jointRows(m.status as never)[0]?.target)) > 2200 && Number.parseInt(String(jointRows(m.status as never)[0]?.target)) <= 3457), "the goal in the joints, stepping toward 3457");
110 expect(p.log.find((e) => e.kind === "state" && e.what === "driver")?.detail).toEqual({ driving: "stream", armSeq: expect.any(Number) });
110 expect(p.log.find((e) => e.kind === "state" && e.what === "driver" && e.detail.driving === "stream")?.detail).toEqual({ driving: "stream", armSeq: expect.any(Number) });
111111 follow.close();
112112 p.ws.close();
113113 });
Software/so101/src/server.ts +45 −4
@@ -70,6 +70,7 @@ import { dirname, extname, join, resolve } from "node:path";
7070 import type { Duplex } from "node:stream";
7171 import { createHash, randomUUID } from "node:crypto";
7272 import { fileURLToPath } from "node:url";
73 import { DEFAULT_GAINS, applyGains, validateGains, type Gains } from "./servo-gains.ts";
7374 import { ParamsError, factoryValues, readServoParams, recordFactory, validateWrite, writeServoParams } from "./servo-params.ts";
7475 import { WebSocketServer, type WebSocket } from "ws";
7576 import { browserModule } from "./browser-modules.ts";
@@ -136,9 +137,11 @@ export interface Settings extends SafetySettings {
136137 tableOffsetMm: number;
137138 /** Per joint, degrees added to the model's angle, to trim the model onto the real arm. */
138139 jointOffsetsDeg: Record<string, number>;
140 /** Per joint, the P/I/D the arm server writes to the servo at start and when a servo comes back; the factory ones are kept beside the settings. */
141 servoGains: Record<string, Gains>;
139142 }
140143
141 export const DEFAULT_SETTINGS: Settings = { zone: "warn", floorMarginMm: 15, baseMarginMm: 10, tableOffsetMm: 0, jointOffsetsDeg: {}, ...DEFAULT_SAFETY };
144 export const DEFAULT_SETTINGS: Settings = { zone: "warn", floorMarginMm: 15, baseMarginMm: 10, tableOffsetMm: 0, jointOffsetsDeg: {}, servoGains: { ...DEFAULT_GAINS }, ...DEFAULT_SAFETY };
142145
143146 /** An emergency stop in force: who hit it and when. It holds until an admin clears it, across restarts. */
144147 export interface EStop {
@@ -210,11 +213,13 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
210213 const machine = new MachineLog();
211214 const run = randomUUID();
212215 const goals = new Map<number, number>();
216 let tuning = false;
213217 /** Torque_Enable sits apart from the present block, so it is read at most every TORQUE_EVERY_MS, and at once after it is written. */
214218 let torque = new Map<number, number>();
215219 let torqueAt = -Infinity;
216220 bus.onSyncWrite = (register, values) => {
217 if (register === STS3215.Goal_Position) for (const [id, v] of values) goals.set(id, v), tracking?.wrote(id, Date.now());
221 // Holding a joint where it rests while its gains are written is not a command: it is no target the page should show.
222 if (register === STS3215.Goal_Position && !tuning) for (const [id, v] of values) goals.set(id, v), tracking?.wrote(id, Date.now());
218223 if (register === STS3215.Torque_Enable) torqueAt = -Infinity;
219224 machine.note("sent", registerName(register), { values: perServo(values) });
220225 };
@@ -552,6 +557,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
552557 }
553558 return [...new Set(value as number[])];
554559 };
560
555561 function validateSettings(s: Partial<Settings>): Settings {
556562 let safety: SafetySettings;
557563 try {
@@ -561,6 +567,13 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
561567 throw err;
562568 }
563569 const out: Settings = { ...DEFAULT_SETTINGS, ...safety, jointOffsetsDeg: {} };
570 if (s.servoGains !== undefined) {
571 try {
572 out.servoGains = validateGains(s.servoGains);
573 } catch (err) {
574 throw new HttpError(400, err instanceof Error ? err.message : String(err));
575 }
576 }
564577 if (s.zone !== undefined) {
565578 if (s.zone !== "enforce" && s.zone !== "warn" && s.zone !== "off") throw new HttpError(400, 'zone must be "enforce", "warn" or "off"');
566579 out.zone = s.zone;
@@ -585,6 +598,31 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
585598 return out;
586599 }
587600
601 /** Servos whose gains are not known to be settings.servoGains: all at start, and any that stopped answering. */
602 const untuned = new Set<number>(ids);
603 /** Write the gains to the servos that lack them, while nobody drives; a busy or stopped arm is tried again at the next status. */
604 async function tuneServos(): Promise<void> {
605 if (tuning || driver || estop || gate.held) return;
606 try {
607 claim("move");
608 } catch {
609 return;
610 }
611 tuning = true;
612 try {
613 const present = [...(await bus.syncRead(ids, STS3215.Present_Position)).keys()].filter((id) => untuned.has(id));
614 if (!present.length) return;
615 recordFactory(factoryFile, await readServoParams(bus, ids)); // the baseline is what the servos had before this server wrote to them
616 for (const { id, values } of await applyGains(bus, present, settings.servoGains)) machine.note("sent", "servo gains", { id, ...values });
617 for (const id of present) untuned.delete(id);
618 } catch (err) {
619 console.error(`servo gains: ${err instanceof Error ? err.message : String(err)}`);
620 } finally {
621 release();
622 tuning = false;
623 }
624 }
625
588626 const status = async () => {
589627 // Present_Position (56) through Present_Temperature (63) is one block: one round trip, not one per register.
590628 const block = await bus.syncReadBlock(ids, PRESENT);
@@ -595,6 +633,8 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
595633 torque = await bus.syncRead(ids, STS3215.Torque_Enable);
596634 torqueAt = performance.now();
597635 }
636 for (const id of ids) if (!positions.has(id)) untuned.add(id); // a servo that fell silent may be a new one
637 if (positions.size && ids.some((id) => untuned.has(id) && positions.has(id))) void tuneServos();
598638 const b = bounds();
599639 const q = model ? toQ(positions) : null;
600640 const joints = ids.map((id) => ({
@@ -842,11 +882,11 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
842882 return json(res, 200, { ...settings, model: model !== null });
843883 case "PUT /settings": {
844884 const body = await readBody(req);
845 const merged = (key: "jointOffsetsDeg" | "jointLimitsDeg" | "jointSpeedDegS" | "jointAccelDegS2") => {
885 const merged = (key: "jointOffsetsDeg" | "jointLimitsDeg" | "jointSpeedDegS" | "jointAccelDegS2" | "servoGains") => {
846886 const patch = body[key];
847887 return patch && typeof patch === "object" && !Array.isArray(patch) ? { ...settings[key], ...patch } : (patch ?? settings[key]);
848888 };
849 settings = validateSettings({ ...settings, ...body, jointOffsetsDeg: merged("jointOffsetsDeg"), jointLimitsDeg: merged("jointLimitsDeg"), jointSpeedDegS: merged("jointSpeedDegS"), jointAccelDegS2: merged("jointAccelDegS2") } as Partial<Settings>);
889 settings = validateSettings({ ...settings, ...body, jointOffsetsDeg: merged("jointOffsetsDeg"), jointLimitsDeg: merged("jointLimitsDeg"), jointSpeedDegS: merged("jointSpeedDegS"), jointAccelDegS2: merged("jointAccelDegS2"), servoGains: merged("servoGains") } as Partial<Settings>);
850890 writeAtomic(settingsFile, JSON.stringify(settings, null, 1));
851891 return json(res, 200, { ...settings, model: model !== null });
852892 }
@@ -1577,6 +1617,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
15771617 to.handleUpgrade(req, socket, head, (ws) => to.emit("connection", ws, req));
15781618 });
15791619
1620 await tuneServos();
15801621 await new Promise<void>((resolve) => http.listen(options.port ?? 8101, options.host ?? "0.0.0.0", resolve));
15811622 const address = http.address();
15821623 const port = typeof address === "object" && address ? address.port : options.port ?? 8101;
Software/so101/src/servo-gains.test.ts added +61
@@ -0,0 +1,61 @@
1 import { afterAll, beforeAll, expect, test } from "bun:test";
2 import { mkdtempSync, readFileSync } from "node:fs";
3 import { tmpdir } from "node:os";
4 import { join } from "node:path";
5 import { FeetechBus } from "./bus.ts";
6 import { FakeBus } from "./fake-bus.ts";
7 import { startServer, type RunningServer } from "./server.ts";
8 import { DEFAULT_GAINS, validateGains } from "./servo-gains.ts";
9
10 const sleep = (ms: number) => new Promise((r) => setTimeout(r, ms));
11 let fake: FakeBus;
12 let server: RunningServer;
13 let base: string;
14 const dataDir = mkdtempSync(join(tmpdir(), "so101-gains-"));
15 const reg = (id: number, address: number) => fake.servos.get(id)![address];
16 const P = 21, D = 22, I = 23;
17
18 beforeAll(async () => {
19 fake = new FakeBus([1, 2, 3]);
20 for (const id of [1, 2, 3]) Object.entries({ 21: 16, 22: 32, 23: 0 }).forEach(([a, v]) => (fake.servos.get(id)![Number(a)] = v));
21 server = await startServer(new FeetechBus(fake), { host: "127.0.0.1", port: 0, ids: [1, 2, 3], posesFile: join(dataDir, "poses.json"), watchdogMs: 60 });
22 base = `http://127.0.0.1:${server.port}`;
23 });
24 afterAll(() => Promise.race([server.close(), sleep(500)]));
25
26 test("the arm server writes the measured gains at start and keeps the factory values", async () => {
27 expect([reg(1, P), reg(1, I), reg(1, D)]).toEqual([64, 0, 32]);
28 expect([reg(2, P), reg(2, I)]).toEqual([64, 4]);
29 expect([reg(3, P), reg(3, I)]).toEqual([64, 4]);
30 const rows = (await (await fetch(`${base}/servo-params`)).json()) as Array<{ id: number; registers: Record<string, number> }>;
31 expect(rows.find((r) => r.id === 2)!.registers.P_Coefficient).toBe(64);
32 const factory = JSON.parse(readFileSync(join(dataDir, "servo-factory.json"), "utf8")) as Record<string, Record<string, number>>;
33 expect(factory[1]!.P_Coefficient).toBe(16); // the baseline is what the servo had before this server wrote to it
34 });
35
36 test("a servo that was replaced and answers again is tuned again", async () => {
37 fake.servos.get(2)![P] = 16; // a new servo, factory P
38 fake.servos.get(2)![I] = 0;
39 const original = fake.servos.get(2)!;
40 fake.servos.delete(2);
41 await fetch(`${base}/status`);
42 fake.servos.set(2, original);
43 await fetch(`${base}/status`);
44 await sleep(100);
45 expect([reg(2, P), reg(2, I)]).toEqual([64, 4]);
46 });
47
48 test("gains are checked: joint names and ranges", () => {
49 expect(validateGains(DEFAULT_GAINS)).toEqual(DEFAULT_GAINS);
50 expect(() => validateGains({ elbow: { P: 20 } })).toThrow("no joint");
51 expect(() => validateGains({ elbow_flex: { P: 65 } })).toThrow("P must be");
52 expect(() => validateGains({ elbow_flex: { Torque_Limit: 1000 } })).toThrow("not P, I or D");
53 expect(() => validateGains([])).toThrow();
54 });
55
56 test("settings carry the gains and refuse bad ones", async () => {
57 const settings = (await (await fetch(`${base}/settings`)).json()) as { servoGains: unknown };
58 expect(settings.servoGains).toEqual(DEFAULT_GAINS);
59 const bad = await fetch(`${base}/settings`, { method: "PUT", body: JSON.stringify({ servoGains: { gripper: { P: 99 } } }) });
60 expect(bad.status).toBe(400);
61 });
Software/so101/src/servo-gains.ts added +74
@@ -0,0 +1,74 @@
1 import type { FeetechBus } from "./bus.ts";
2 import { TUNABLE, writeServoParams } from "./servo-params.ts";
3 import { JOINTS, STS3215 } from "./sts3215.ts";
4
5 /** One joint's loop gains; a gain left out stays as the servo has it. */
6 export interface Gains {
7 P?: number;
8 I?: number;
9 D?: number;
10 }
11
12 /**
13 * What the arm is tuned to, measured on the real arm 2026-10-21 (README, "Servo tuning"): the shipped P16 I0 settled
14 * 24-27 ticks short and never arrived; P64 arrives in 150-330 ms with at most 3 ticks of overshoot. The gravity
15 * joints take I4 for the last ticks. The gripper stays at the factory P16: its torque is capped on purpose.
16 */
17 export const DEFAULT_GAINS: Readonly<Record<string, Gains>> = {
18 shoulder_pan: { P: 64 },
19 shoulder_lift: { P: 64, I: 4 },
20 elbow_flex: { P: 64, I: 4 },
21 wrist_flex: { P: 64 },
22 wrist_roll: { P: 64 },
23 };
24
25 const KEYS = ["P", "I", "D"] as const;
26
27 /** `servoGains` as stored or sent: joint name → gains inside the ranges a write may use; throws a message naming the first bad one. */
28 export function validateGains(input: unknown): Record<string, Gains> {
29 if (typeof input !== "object" || input === null || Array.isArray(input)) throw new Error("servoGains must be an object of joint: {P, I, D}");
30 const out: Record<string, Gains> = {};
31 for (const [joint, gains] of Object.entries(input)) {
32 if (!Object.values(JOINTS).includes(joint)) throw new Error(`no joint "${joint}"`);
33 if (typeof gains !== "object" || gains === null || Array.isArray(gains)) throw new Error(`servoGains.${joint} must be an object of P, I, D`);
34 const kept: Gains = {};
35 for (const key of Object.keys(gains)) if (!(KEYS as readonly string[]).includes(key)) throw new Error(`servoGains.${joint}.${key} is not P, I or D`);
36 for (const key of KEYS) {
37 const value = (gains as Gains)[key];
38 if (value === undefined) continue;
39 const { min, max } = TUNABLE[key];
40 if (typeof value !== "number" || !Number.isInteger(value) || value < min || value > max) throw new Error(`servoGains.${joint}.${key} must be a whole number from ${min} to ${max}`);
41 kept[key] = value;
42 }
43 out[joint] = kept;
44 }
45 return out;
46 }
47
48 /**
49 * Write each listed servo the gains it lacks, and only those: a servo already at its gains is not touched, so a
50 * restart costs the EEPROM nothing. Returns what was written, per servo id. The caller holds the arm.
51 */
52 export async function applyGains(bus: FeetechBus, ids: readonly number[], gains: Readonly<Record<string, Gains>>): Promise<Array<{ id: number; values: Record<string, number> }>> {
53 const written: Array<{ id: number; values: Record<string, number> }> = [];
54 for (const id of ids) {
55 const want = gains[JOINTS[id] ?? ""];
56 if (!want) continue;
57 const values: Record<string, number> = {};
58 let silent = false;
59 for (const key of KEYS) {
60 const { register } = TUNABLE[key];
61 if (want[key] === undefined) continue;
62 const now = (await bus.syncRead([id], STS3215[register])).get(id);
63 if (now === undefined) {
64 silent = true; // a servo that does not answer has nothing to write to; the next sighting tries again
65 break;
66 }
67 if (now !== want[key]) values[register] = want[key]!;
68 }
69 if (silent || !Object.keys(values).length) continue;
70 await writeServoParams(bus, id, values);
71 written.push({ id, values });
72 }
73 return written;
74 }
Software/so101/src/ui/intent.js +1 −1
@@ -7,7 +7,7 @@
77 export class Intent {
88 /** @param {{ hold?: number, near?: number }} [options] hold: ms an unreached target is kept after release; near: ticks that count as arrived */
99 constructor(options = {}) {
10 this.hold = options.hold ?? 2000;
10 this.hold = options.hold ?? 1000;
1111 this.near = options.near ?? 12;
1212 /** @type {Map<number, { ticks: number, down: boolean, since: number, hold?: number }>} */
1313 this.targets = new Map();
Software/so101/src/ui/intent.test.ts +3 −3
@@ -14,11 +14,11 @@ describe("Intent", () => {
1414 expect(i.target(1, 2995, 600)).toBeNull();
1515 expect(i.target(1, 2048, 700)).toBeNull();
1616 });
17 test("an arm that never arrives is followed again after 2 s", () => {
17 test("an arm that never arrives is followed again after 1 s, three times the slowest settle measured (330 ms)", () => {
1818 const i = new Intent();
1919 i.set(2, 3000, 0); i.release(2, 0);
20 expect(i.target(2, 2048, 1999)).toBe(3000);
21 expect(i.target(2, 2048, 2000)).toBeNull();
20 expect(i.target(2, 2048, 999)).toBe(3000);
21 expect(i.target(2, 2048, 1000)).toBeNull();
2222 });
2323 test("a held target is not dropped when the arm happens to be there", () => {
2424 const i = new Intent();

↑↓ navigate ↵ open ⌘↵ its app esc close

Keyboard shortcuts

Docs