119 lines
3.4 KiB
Python
119 lines
3.4 KiB
Python
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):
|
|
command = bytearray((0xC3, 0x3C, mode, 0))
|
|
command.append(sum(command) & 0xFF)
|
|
ser.write(command)
|
|
|
|
|
|
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()
|