Feat: add ESP32 bridge functionality and physical switch control
This commit is contained in:
Brunsmeier
132
main.py
132
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()
|
||||
|
||||
Reference in New Issue
Block a user