From e2c1597b68321ba43c172b98b29a5dda0d1f5203 Mon Sep 17 00:00:00 2001 From: Brunsmeier <2970937094@qq.com> Date: Fri, 26 Jun 2026 13:06:27 +0800 Subject: [PATCH] Feat: add ESP32 bridge functionality and physical switch control --- .gitignore | 6 ++ esp_bridge.py | 209 ++++++++++++++++++++++++++++++++++++++++++++++++++ main.py | 132 ++++++++++++++++++++++++------- pc_reader.py | 37 ++++++++- 4 files changed, 355 insertions(+), 29 deletions(-) create mode 100644 .gitignore create mode 100644 esp_bridge.py diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..d2d51f6 --- /dev/null +++ b/.gitignore @@ -0,0 +1,6 @@ +__pycache__/ +*.py[cod] +*.class + + +.vscode/ diff --git a/esp_bridge.py b/esp_bridge.py new file mode 100644 index 0000000..0094b46 --- /dev/null +++ b/esp_bridge.py @@ -0,0 +1,209 @@ +import struct +import time +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), + } + + +class ESP32Bridge: + def __init__( + self, + port, + baud=115200, + timeout=0.05, + startup_wait=1.0, + safe_stop_on_close=True, + reset_input_on_open=True, + reset_output_on_open=True, + ): + self.port = port + self.baud = baud + self.timeout = timeout + self.startup_wait = startup_wait + self.safe_stop_on_close = safe_stop_on_close + self.reset_input_on_open = reset_input_on_open + self.reset_output_on_open = reset_output_on_open + + self.ser = None + self.buffer = bytearray() + self.last_t_ms = None + + def is_open(self): + return self.ser is not None and self.ser.is_open + + def _require_open(self): + if not self.is_open(): + raise RuntimeError("Serial port is not open.") + + def open(self): + if self.is_open(): + return self + + self.ser = serial.Serial() + self.ser.port = self.port + self.ser.baudrate = self.baud + self.ser.timeout = self.timeout + self.ser.write_timeout = self.timeout + + # Avoid ESP32 auto reset / boot mode weirdness. + self.ser.dtr = False + self.ser.rts = False + self.ser.open() + + self.ser.dtr = False + self.ser.rts = False + + if self.reset_input_on_open: + self.ser.reset_input_buffer() + + if self.reset_output_on_open: + self.ser.reset_output_buffer() + + self.buffer.clear() + self.last_t_ms = None + + if self.startup_wait > 0: + time.sleep(self.startup_wait) + + return self + + def close(self): + if self.ser is None: + return + + if self.safe_stop_on_close and self.ser.is_open: + try: + self.stop_gripper() + time.sleep(0.1) + except Exception: + pass + + try: + if self.ser.is_open: + self.ser.close() + finally: + self.ser = None + + def __enter__(self): + return self.open() + + def __exit__(self, exc_type, exc, tb): + self.close() + + def send_gripper_command(self, mode): + self._require_open() + + if mode == 1: + self.ser.write(b"o") + elif mode == 2: + self.ser.write(b"c") + else: + self.ser.write(b"s") + + self.ser.flush() + + def open_gripper(self): + self.send_gripper_command(1) + + def close_gripper(self): + self.send_gripper_command(2) + + def stop_gripper(self): + self.send_gripper_command(0) + + def read_packet(self): + """ + Return one decoded packet dict. + This blocks until a valid packet is found. + """ + self._require_open() + + while True: + chunk = self.ser.read(self.ser.in_waiting or 1) + if chunk: + self.buffer.extend(chunk) + + packet = self._try_pop_packet() + if packet is not None: + return decode_packet(packet) + + def _try_pop_packet(self): + while len(self.buffer) >= PACKET_SIZE: + sync_index = self.buffer.find(SYNC) + + if sync_index < 0: + # Keep last byte in case it is the first half of SYNC. + del self.buffer[:-1] + return None + + if sync_index: + del self.buffer[:sync_index] + + if len(self.buffer) < PACKET_SIZE: + return None + + packet = bytes(self.buffer[:PACKET_SIZE]) + + if (sum(packet[:-1]) & 0xFF) != packet[-1]: + del self.buffer[0] + continue + + del self.buffer[:PACKET_SIZE] + return packet + + return None + + def read_packet_with_frequency(self): + data = self.read_packet() + + if self.last_t_ms is None: + freq_hz = 0.0 + else: + delta = (data["t_ms"] - self.last_t_ms) & 0xFFFFFFFF + freq_hz = 1000.0 / delta if delta else 0.0 + + self.last_t_ms = data["t_ms"] + data["freq_hz"] = freq_hz + return data + + def iter_packets(self, seconds=None, max_samples=None, with_frequency=True): + start = time.monotonic() + count = 0 + + while True: + if seconds is not None and time.monotonic() - start >= seconds: + break + + if max_samples is not None and count >= max_samples: + break + + if with_frequency: + yield self.read_packet_with_frequency() + else: + yield self.read_packet() + + count += 1 \ No newline at end of file diff --git a/main.py b/main.py index 8646cd3..4d88fc7 100644 --- a/main.py +++ b/main.py @@ -13,6 +13,14 @@ import select # IMU RX green -> ESP32 GPIO17 / TX2 # IMU GND -> ESP32 GND # +# Physical switch: +# GPIO27 S -> toggle switch one side +# GPIO27 G -> toggle switch other side +# +# GPIO27 switch behavior: +# OFF -> ON : CLOSE pulse +# ON -> OFF : OPEN pulse +# ============================================================ # UART2: IMU # USB stdout/stdin: PC # @@ -20,8 +28,6 @@ import select # - Output uses sys.stdout, because this is the stable path. # - Command input uses sys.stdin, ASCII only: o / c / s. # - First test with USE_REAL_DOUT = False. -# ============================================================ - # ---------------- PIN CONFIG ---------------- @@ -32,19 +38,16 @@ DIN_PINS = (25, 26, 27, 32) DOUT_PINS = (18, 19) digital_inputs = [Pin(pin, Pin.IN, Pin.PULL_UP) for pin in DIN_PINS] + # Low-level trigger relay: # GPIO HIGH = relay OFF # GPIO LOW = relay ON digital_outputs = [Pin(pin, Pin.OUT, value=1) for pin in DOUT_PINS] + # ---------------- SERIAL CONFIG ---------------- -# PC side uses MicroPython stdout/stdin. -# PC reader should use: -# python3 pc_reader.py /dev/ttyUSB0 --baud 115200 PC_BAUD_NOTE = 115200 - -# IMU side confirmed working. IMU_BAUD = 921600 SEND_PERIOD_MS = 20 # 50 Hz @@ -67,15 +70,6 @@ raw_values = [0] * 10 # ---------------- DOUT TEST MODE ---------------- -# False: -# Only update software DOUT state in outgoing packet. -# GPIO18/GPIO19 will NOT really change. -# -# True: -# Actually drive GPIO18/GPIO19. -# -# Start with False. If o/c/s changes DOUT in pc_reader output, -# then set it to True and test real hardware. USE_REAL_DOUT = True # bit0 = DOUT0 / GPIO18 @@ -83,6 +77,30 @@ USE_REAL_DOUT = True dout_shadow = 0 +# ---------------- PHYSICAL SWITCH CONFIG ---------------- + +# DIN_PINS = (25, 26, 27, 32) +# GPIO27 is index 2. +SWITCH_INDEX = 2 + +# How long each switch movement triggers the relay. +# If the cutter movement is too short, increase to 500 / 800. +SWITCH_PULSE_MS = 2000 + +# Debounce time for mechanical toggle switch. +SWITCH_DEBOUNCE_MS = 80 + +# Initial switch state. +# PULL_UP: +# switch open = 1 +# switch closed = 0 +switch_last_value = digital_inputs[SWITCH_INDEX].value() +switch_last_edge_ms = time.ticks_ms() + +switch_pulse_active = False +switch_pulse_start_ms = time.ticks_ms() + + # ---------------- BUFFERS ---------------- rx_buffer = bytearray(2048) @@ -104,6 +122,7 @@ poller.register(sys.stdin, select.POLLIN) # ============================================================ # DOUT / gripper # ============================================================ + def clear_douts(): global dout_shadow @@ -117,6 +136,7 @@ def clear_douts(): for pin in digital_outputs: pin.value(1) + def set_gripper(mode): global dout_shadow @@ -153,6 +173,7 @@ def set_gripper(mode): return # First turn both relays OFF. + # This prevents IN1 and IN2 being active at the same time. digital_outputs[0].value(1) # GPIO18 / IN1 OFF digital_outputs[1].value(1) # GPIO19 / IN2 OFF time.sleep_ms(20) @@ -168,6 +189,56 @@ def set_gripper(mode): # mode 0 keeps both HIGH/OFF +# ============================================================ +# Physical switch control +# ============================================================ + +def start_switch_pulse(mode): + global switch_pulse_active, switch_pulse_start_ms + + set_gripper(mode) + switch_pulse_active = True + switch_pulse_start_ms = time.ticks_ms() + + +def poll_physical_switch(): + global switch_last_value, switch_last_edge_ms + + now = time.ticks_ms() + value = digital_inputs[SWITCH_INDEX].value() # GPIO27 + + # Detect state change with debounce. + if value != switch_last_value: + if time.ticks_diff(now, switch_last_edge_ms) > SWITCH_DEBOUNCE_MS: + old_value = switch_last_value + switch_last_value = value + switch_last_edge_ms = now + + # OFF -> ON: + # GPIO27 goes 1 -> 0 + # switch connects S to G + if old_value == 1 and value == 0: + start_switch_pulse(2) # CLOSE + + # ON -> OFF: + # GPIO27 goes 0 -> 1 + elif old_value == 0 and value == 1: + start_switch_pulse(1) # OPEN + + +def update_switch_pulse_stop(): + global switch_pulse_active + + if not switch_pulse_active: + return + + now = time.ticks_ms() + + if time.ticks_diff(now, switch_pulse_start_ms) >= SWITCH_PULSE_MS: + set_gripper(0) # STOP + switch_pulse_active = False + + # ============================================================ # IMU UART # ============================================================ @@ -187,9 +258,14 @@ def open_imu_uart(): time.sleep_ms(300) - # Flush startup junk. - while uart.any(): - uart.read() + # Flush startup junk, but do NOT wait forever. + # IMU is a continuous stream; an unlimited while uart.any() may get stuck. + t0 = time.ticks_ms() + while time.ticks_diff(time.ticks_ms(), t0) < 100: + if uart.any(): + uart.read() + else: + time.sleep_ms(1) return uart @@ -269,11 +345,6 @@ def make_packet(): for index, pin in enumerate(digital_inputs): din_mask |= pin.value() << index - # if USE_REAL_DOUT: - # dout_mask = 0 - # for index, pin in enumerate(digital_outputs): - # dout_mask |= pin.value() << index - # else: dout_mask = dout_shadow # Packet layout: @@ -293,6 +364,10 @@ def make_packet(): def usb_write(data): usb_out.write(data) + try: + usb_out.flush() + except AttributeError: + pass # ============================================================ @@ -300,13 +375,13 @@ def usb_write(data): # ============================================================ def poll_usb_commands(): + global switch_pulse_active + # ASCII only. # pc_reader.py should send: # b"o" = OPEN # b"c" = CLOSE # b"s" = STOP - # - # Do NOT use C3 3C binary command here. for _ in range(8): if not poller.poll(0): @@ -318,12 +393,15 @@ def poll_usb_commands(): break if ch == "o" or ch == "O": + switch_pulse_active = False set_gripper(1) elif ch == "c" or ch == "C": + switch_pulse_active = False set_gripper(2) elif ch == "s" or ch == "S": + switch_pulse_active = False set_gripper(0) # Ignore Enter/newline and other characters. @@ -345,6 +423,8 @@ last_send_ms = time.ticks_ms() while True: poll_usb_commands() + poll_physical_switch() + update_switch_pulse_stop() poll_imu(imu_uart) now = time.ticks_ms() diff --git a/pc_reader.py b/pc_reader.py index 94b761b..2464492 100644 --- a/pc_reader.py +++ b/pc_reader.py @@ -55,17 +55,41 @@ def keyboard_control(ser): def main(): + import time + parser = argparse.ArgumentParser() - parser.add_argument("port", help="ESP32 serial port, for example COM5") - parser.add_argument("--baud", type=int, default=230400) + parser.add_argument("port", help="ESP32 serial port, for example /dev/ttyUSB0") + parser.add_argument("--baud", type=int, default=115200) args = parser.parse_args() buffer = bytearray() packet_count = 0 last_time_ms = None - with serial.Serial(args.port, args.baud, timeout=1) as ser: + ser = serial.Serial() + ser.port = args.port + ser.baudrate = args.baud + ser.timeout = 0.05 + ser.write_timeout = 0.05 + + # Avoid unwanted ESP32 auto-reset / boot-mode toggling. + ser.dtr = False + ser.rts = False + + ser.open() + + try: + # Some USB-UART adapters change line states during open(). + ser.dtr = False + ser.rts = False + + ser.reset_input_buffer() + ser.reset_output_buffer() + print("Reading {} at {} baud".format(args.port, args.baud)) + print("Waiting for ESP32 main.py startup...") + time.sleep(4.0) + threading.Thread( target=keyboard_control, args=(ser,), @@ -82,12 +106,15 @@ def main(): 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 @@ -101,6 +128,7 @@ def main(): 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( @@ -116,6 +144,9 @@ def main(): ) ) + finally: + ser.close() + if __name__ == "__main__": main()