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

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.ino
index 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.md
new file mode 100644
index 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.sh
new file mode 100644
index 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.py
new file mode 100644
index 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.py
new file mode 100644
index 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.py
new file mode 100644
index 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.sh
new file mode 100644
index 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)"