real field test

2026-06-18 16:55:30 +08:00
commit 55aea83cbe
5 changed files with 671 additions and 0 deletions
--- a/pc_reader.py
+++ b/pc_reader.py
@ -0,0 +1,121 @@
+import argparse
+import struct
+import threading
+
+import serial
+
+
+SYNC = b"\xA5\x5A"
+PACKET_FORMAT = "<2sBBBI10hB"
+PACKET_SIZE = struct.calcsize(PACKET_FORMAT)
+
+
+def decode_packet(packet):
+    fields = struct.unpack(PACKET_FORMAT, packet)
+    _, version, din_mask, dout_mask, time_ms, *rest = fields
+    raw = rest[:-1]
+
+    acc_scale = 16.0 / 32768.0
+    gyro_scale = 2000.0 / 32768.0
+    angle_scale = 180.0 / 32768.0
+
+    return {
+        "version": version,
+        "t_ms": time_ms,
+        "din": [(din_mask >> bit) & 1 for bit in range(4)],
+        "dout": [(dout_mask >> bit) & 1 for bit in range(2)],
+        "acc_g": [round(value * acc_scale, 5) for value in raw[0:3]],
+        "gyro_dps": [round(value * gyro_scale, 4) for value in raw[3:6]],
+        "angle_deg": [round(value * angle_scale, 4) for value in raw[6:9]],
+        "temp_c": round(raw[9] / 100.0, 2),
+    }
+
+
+def send_gripper_command(ser, mode):
+    if mode == 1:
+        ser.write(b"o")
+    elif mode == 2:
+        ser.write(b"c")
+    else:
+        ser.write(b"s")
+
+
+def keyboard_control(ser):
+    print("Commands: o=OPEN, c=CLOSE, s=STOP")
+    while True:
+        text = input().strip().lower()
+        if text == "o":
+            send_gripper_command(ser, 1)
+        elif text == "c":
+            send_gripper_command(ser, 2)
+        elif text == "s":
+            send_gripper_command(ser, 0)
+        else:
+            print("Unknown command. Use o, c, or s.")
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("port", help="ESP32 serial port, for example COM5")
+    parser.add_argument("--baud", type=int, default=230400)
+    args = parser.parse_args()
+
+    buffer = bytearray()
+    packet_count = 0
+    last_time_ms = None
+
+    with serial.Serial(args.port, args.baud, timeout=1) as ser:
+        print("Reading {} at {} baud".format(args.port, args.baud))
+        threading.Thread(
+            target=keyboard_control,
+            args=(ser,),
+            daemon=True,
+        ).start()
+
+        while True:
+            chunk = ser.read(ser.in_waiting or 1)
+            if chunk:
+                buffer.extend(chunk)
+
+            while len(buffer) >= PACKET_SIZE:
+                sync_index = buffer.find(SYNC)
+                if sync_index < 0:
+                    del buffer[:-1]
+                    break
+                if sync_index:
+                    del buffer[:sync_index]
+                if len(buffer) < PACKET_SIZE:
+                    break
+
+                packet = bytes(buffer[:PACKET_SIZE])
+                if (sum(packet[:-1]) & 0xFF) != packet[-1]:
+                    del buffer[0]
+                    continue
+
+                del buffer[:PACKET_SIZE]
+                data = decode_packet(packet)
+                packet_count += 1
+
+                if last_time_ms is None:
+                    frequency = 0.0
+                else:
+                    delta = (data["t_ms"] - last_time_ms) & 0xFFFFFFFF
+                    frequency = 1000.0 / delta if delta else 0.0
+                last_time_ms = data["t_ms"]
+
+                print(
+                    "#{:8d} {:6.1f}Hz DIN={} DOUT={} "
+                    "ACC={} GYRO={} ANGLE={}".format(
+                        packet_count,
+                        frequency,
+                        data["din"],
+                        data["dout"],
+                        data["acc_g"],
+                        data["gyro_dps"],
+                        data["angle_deg"],
+                    )
+                )
+
+
+if __name__ == "__main__":
+    main()