MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.
Python
firmware V0.4 on hardware: telemetry was dropped by the CDC FIFO guard (fixed, v2); bench scripts (flash, motor test, servo scan and watch)
metroxa90ec51e4528
diff --git a/firmware/MOSS_QTPY_MDD3A/MOSS_QTPY_MDD3A.ino b/firmware/MOSS_QTPY_MDD3A/MOSS_QTPY_MDD3A.inoindex 6915a32..590b7d2 100644--- a/firmware/MOSS_QTPY_MDD3A/MOSS_QTPY_MDD3A.ino+++ b/firmware/MOSS_QTPY_MDD3A/MOSS_QTPY_MDD3A.ino@@ -26,7 +26,7 @@ constexpr bool INVERT_LEFT = false, INVERT_RIGHT = false;constexpr float SHUNT_OHMS = 0.0f; // voltage only (see V0.3 README)constexpr uint32_t PWM_HZ = 20000;constexpr int PWM_BITS = 10;-constexpr const char* FW = "qtpy-mdd3a-v1";+constexpr const char* FW = "qtpy-mdd3a-v2";moss::Control control;moss::Ramp rampL, rampR;@@ -54,11 +54,12 @@ void ARDUINO_ISR_ATTR encoderRight() {portEXIT_CRITICAL_ISR(&encoderMux);}void sendLine(const char* text) {- size_t n = strlen(text);- // Drop a message rather than block motor/watchdog processing on a stalled host.- if (Serial.availableForWrite() >= int(n + 1)) {- Serial.write((const uint8_t*)text, n); Serial.write('\n');- }+ // First hardware contact: the native CDC FIFO is 64 bytes, so a guard that required the+ // whole line to fit dropped every telemetry line (~330 bytes). write() already returns 0 when no+ // host has the port open (DTR low); with a 20 ms tx timeout a stalled host costs at most 20 ms per+ // line, which keeps the deadman (300 ms) serviced. A line cut by that timeout is the host's problem+ // (one bad JSON line), never the motors'.+ Serial.write((const uint8_t*)text, strlen(text)); Serial.write('\n');}void outputsZero() {if (pwmOK) { ledcWrite(M1A, 0); ledcWrite(M1B, 0); ledcWrite(M2A, 0); ledcWrite(M2B, 0); }@@ -152,7 +153,7 @@ void serviceSerial() {void setup() {for (int pin : {M1A, M1B, M2A, M2B}) { pinMode(pin, OUTPUT); digitalWrite(pin, LOW); }Serial.begin(115200);- Serial.setTxTimeoutMs(0); // native USB: never block on a host that stopped reading+ Serial.setTxTimeoutMs(20); // native USB: bounded wait, see sendLine()bool ok = true;for (int pin : {M1A, M1B, M2A, M2B}) ok = ledcAttach(pin, PWM_HZ, PWM_BITS) && ok;pwmOK = ok;diff --git a/firmware/MOSS_QTPY_MDD3A/bench/README.md b/firmware/MOSS_QTPY_MDD3A/bench/README.mdnew file mode 100644index 0000000..fd5e230--- /dev/null+++ b/firmware/MOSS_QTPY_MDD3A/bench/README.md@@ -0,0 +1,8 @@+# Bench scripts for the V0.4 board++Run from the onboard computer the QT Py is plugged into. They need `pyserial` and `esptool` in the Python used (`PY` at the top of `flash_qtpy.sh`).++- `flash_qtpy.sh`: puts the QT Py in its ROM bootloader through the native USB, writes the four images produced by `arduino-cli compile --fqbn esp32:esp32:adafruit_qtpy_esp32s3_nopsram:CDCOnBoot=cdc --output-dir <dir>` (plus `boot_app0.bin` from the ESP32 core), then resets the chip with the RTC watchdog. A plain RTS reset leaves the chip in the bootloader, the watchdog reset does not.+- `moss_bench.py "test left 15"`: sends one command and prints the telemetry lines that follow (mode, output, encoder ticks, battery). Start with `test left 15`, then `test right 15`, rover on blocks. At 5 % the motors do not start.+- `servo_scan.py`: pings every ID on the arm's servo bus (Feetech SMS_STS, 1 Mbps) and reads model, position, voltage and temperature of each servo that answers.+- `servo_watch.py` / `watch_start.sh`: logs which servo moves while you move the arm by hand (torque off), to map IDs to joints before assigning them.diff --git a/firmware/MOSS_QTPY_MDD3A/bench/flash_qtpy.sh b/firmware/MOSS_QTPY_MDD3A/bench/flash_qtpy.shnew file mode 100644index 0000000..e046c12--- /dev/null+++ b/firmware/MOSS_QTPY_MDD3A/bench/flash_qtpy.sh@@ -0,0 +1,26 @@+#!/bin/bash+# Flash the MOSS V0.4 firmware on the QT Py ESP32-S3 from the Jetson, through its native USB.+# Step 1: ask the running Arduino app to reboot into the ROM bootloader (DTR/RTS trick);+# step 2: write; step 3: reset out of the bootloader by toggling RTS with the port held open.+set -e+PY=~/vector-dimos/.venv/bin/python+cd ~/moss_fw_v04+port() { ls /dev/serial/by-id/ | grep -E "QT_Py|Espressif" | head -1 | sed 's|^|/dev/serial/by-id/|'; }+P=$(port)+if [[ "$P" == *QT_Py* ]]; then+ $PY -m esptool --chip esp32s3 --port "$P" --before default-reset --after no-reset chip-id >/dev/null 2>&1 || true+ sleep 3; P=$(port)+fi+echo "bootloader port: $P"+$PY -m esptool --chip esp32s3 --port "$P" --baud 921600 --before no-reset --after watchdog-reset write-flash -z \+ --flash-mode dio --flash-freq 80m --flash-size 8MB \+ 0x0 MOSS_QTPY_MDD3A.ino.bootloader.bin 0x8000 MOSS_QTPY_MDD3A.ino.partitions.bin 0xe000 boot_app0.bin 0x10000 MOSS_QTPY_MDD3A.ino.bin 2>&1 | grep -E "Wrote|verified|rror"+$PY - <<PYEOF+import serial, time+s = serial.Serial("$P", 115200, timeout=0.1)+s.dtr = False; s.rts = True; time.sleep(0.1); s.rts = False+try:+ time.sleep(0.5); s.close()+except Exception: pass+PYEOF+sleep 4; ls /dev/serial/by-id/ | grep -E "QT_Py|Espressif"diff --git a/firmware/MOSS_QTPY_MDD3A/bench/moss_bench.py b/firmware/MOSS_QTPY_MDD3A/bench/moss_bench.pynew file mode 100644index 0000000..7ef7f2c--- /dev/null+++ b/firmware/MOSS_QTPY_MDD3A/bench/moss_bench.py@@ -0,0 +1,27 @@+"""First contact bench for the MOSS V0.4 base: send one command, log every telemetry line, summarise.+Usage: python moss_bench.py "test left 5" [seconds]+"""+import glob, json, sys, time, serial++cmd = sys.argv[1]+secs = float(sys.argv[2]) if len(sys.argv) > 2 else 2.5+port = [p for p in glob.glob("/dev/serial/by-id/*") if "QT_Py" in p][0]+s = serial.Serial(port, 115200, timeout=0.05)+time.sleep(0.3); s.reset_input_buffer()+s.write(b"zero\n"); time.sleep(0.3); s.reset_input_buffer()+s.write((cmd + "\n").encode())+t0 = time.time(); rows = []; other = []+buf = b""+while time.time() - t0 < secs:+ buf += s.read(4096)+ while b"\n" in buf:+ line, buf = buf.split(b"\n", 1)+ try:+ rows.append((time.time() - t0, json.loads(line)))+ except Exception:+ other.append(line.decode("utf8", "replace"))+s.close()+print("replies:", other[:5])+print(f"{len(rows)} telemetry lines in {secs} s")+for t, r in rows[::2]:+ print(f"{t:4.2f}s mode={r['mode']} {r['reason']:<12} out={r['output_pct']} ticks={r['ticks']} bad={r['invalid_edges']} bus={r['bus_mV']} mV")diff --git a/firmware/MOSS_QTPY_MDD3A/bench/servo_scan.py b/firmware/MOSS_QTPY_MDD3A/bench/servo_scan.pynew file mode 100644index 0000000..fa26f59--- /dev/null+++ b/firmware/MOSS_QTPY_MDD3A/bench/servo_scan.py@@ -0,0 +1,42 @@+"""Scan a Feetech (SMS_STS) servo bus: ping every ID at 1 Mbps, then read model/position/voltage of each.+Usage: python servo_scan.py [port] [baud]+"""+import glob, sys, time, serial++port = sys.argv[1] if len(sys.argv) > 1 else [p for p in glob.glob("/dev/serial/by-id/*") if "Single_Serial" in p][0]+baud = int(sys.argv[2]) if len(sys.argv) > 2 else 1_000_000+s = serial.Serial(port, baud, timeout=0.02)++def packet(sid, instr, params=b""):+ body = bytes([sid, len(params) + 2, instr]) + params+ return b"\xff\xff" + body + bytes([(~sum(body)) & 0xFF])++def txrx(sid, instr, params=b"", want=6, wait=0.02):+ s.reset_input_buffer()+ pkt = packet(sid, instr, params)+ s.write(pkt); s.flush()+ t0 = time.time(); buf = b""+ while time.time() - t0 < wait:+ buf += s.read(64)+ if len(buf) >= len(pkt) + want: break+ if buf.startswith(pkt): buf = buf[len(pkt):] # half-duplex echo+ i = buf.find(b"\xff\xff")+ return buf[i:] if i >= 0 else b""++def read_reg(sid, addr, n):+ r = txrx(sid, 0x02, bytes([addr, n]), want=6 + n)+ if len(r) >= 6 + n and r[2] == sid:+ return r[5:5 + n]+ return None++found = []+for sid in range(0, 254):+ r = txrx(sid, 0x01)+ if len(r) >= 6 and r[2] == sid:+ found.append((sid, r[4]))+print(f"port {port} @ {baud}: {len(found)} servo(s) answer:", [f for f, _ in found])+for sid, err in found:+ model = read_reg(sid, 3, 2); pos = read_reg(sid, 56, 2); volt = read_reg(sid, 62, 1); temp = read_reg(sid, 63, 1); torque = read_reg(sid, 40, 1)+ f = lambda b: int.from_bytes(b, "little") if b else None+ print(f" ID {sid:3d} model={f(model)} pos={f(pos)} volt={volt[0]/10 if volt else None} V temp={temp[0] if temp else None} C torque_on={torque[0] if torque else None} err={err}")+s.close()diff --git a/firmware/MOSS_QTPY_MDD3A/bench/servo_watch.py b/firmware/MOSS_QTPY_MDD3A/bench/servo_watch.pynew file mode 100644index 0000000..50d351d--- /dev/null+++ b/firmware/MOSS_QTPY_MDD3A/bench/servo_watch.py@@ -0,0 +1,29 @@+"""Log which servo moves while someone moves the arm by hand (torque off). Ctrl-C or kill to stop.+Writes one line per movement event to the log path given as argv[1]."""+import glob, sys, time, serial+log = open(sys.argv[1], "a", buffering=1)+ids = [int(x) for x in sys.argv[2:]]+port = [p for p in glob.glob("/dev/serial/by-id/*") if "Single_Serial" in p][0]+s = serial.Serial(port, 1_000_000, timeout=0.01)+def packet(sid, instr, params=b""):+ body = bytes([sid, len(params) + 2, instr]) + params+ return b"\xff\xff" + body + bytes([(~sum(body)) & 0xFF])+def pos(sid):+ s.reset_input_buffer(); pkt = packet(sid, 0x02, bytes([56, 2])); s.write(pkt); s.flush()+ t0=time.time(); buf=b""+ while time.time()-t0 < 0.015: buf += s.read(64)+ if buf.startswith(pkt): buf = buf[len(pkt):]+ if len(buf) >= 8 and buf[2]==sid: return int.from_bytes(buf[5:7], "little")+ return None+base = {i: pos(i) for i in ids}+last = dict(base); lastlog = {i: 0 for i in ids}+log.write(f"# start {time.strftime('%H:%M:%S')} base={base}\n")+t0=time.time()+while True:+ for i in ids:+ p = pos(i)+ if p is None or last[i] is None: last[i] = p if p is not None else last[i]; continue+ if abs(p - last[i]) >= 25 and time.time() - lastlog[i] > 0.5:+ log.write(f"{time.time()-t0:7.1f}s ID {i:3d} moved {last[i]} -> {p}\n"); lastlog[i] = time.time()+ last[i] = p+ time.sleep(0.02)diff --git a/firmware/MOSS_QTPY_MDD3A/bench/watch_start.sh b/firmware/MOSS_QTPY_MDD3A/bench/watch_start.shnew file mode 100644index 0000000..dafaff7--- /dev/null+++ b/firmware/MOSS_QTPY_MDD3A/bench/watch_start.sh@@ -0,0 +1,8 @@+#!/bin/bash+# Start the servo watcher in the background (kill any previous one by pidfile, never by pattern).+cd ~/moss_fw_v04+[ -f watch.pid ] && kill "$(cat watch.pid)" 2>/dev/null+rm -f watch.log+nohup ~/vector-dimos/.venv/bin/python servo_watch.py watch.log "$@" >watch.err 2>&1 &+echo $! > watch.pid+sleep 2; cat watch.log; echo "pid $(cat watch.pid) alive: $(kill -0 $(cat watch.pid) 2>/dev/null && echo yes || echo no)"