Feat: add ESP32 bridge functionality and physical switch control

.gitignore

@@ -0,0 +1,6 @@
+__pycache__/
+*.py[cod]
+*.class
+
+
+.vscode/

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

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.
+    # Flush startup junk, but do NOT wait forever.
-    while uart.any():
+    # IMU is a continuous stream; an unlimited while uart.any() may get stuck.
-        uart.read()
+    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()

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("port", help="ESP32 serial port, for example /dev/ttyUSB0")
-    parser.add_argument("--baud", type=int, default=230400)
+    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()