324 lines
9.4 KiB
Python
324 lines
9.4 KiB
Python
from machine import Pin, ADC
|
||
import time, network, espnow
|
||
|
||
# ---------------- ESP-NOW helpers ----------------
|
||
_espnow_sta = None
|
||
_espnow = None
|
||
_espnow_peers = []
|
||
_espnow_broadcast = b'\xff\xff\xff\xff\xff\xff'
|
||
|
||
led = Pin(15, Pin.OUT);
|
||
led.value(0)
|
||
btn2 = Pin(2, Pin.IN, Pin.PULL_DOWN)
|
||
btn5 = Pin(5, Pin.IN, Pin.PULL_DOWN)
|
||
btn6 = Pin(6, Pin.IN, Pin.PULL_DOWN)
|
||
btn7 = Pin(7, Pin.IN, Pin.PULL_DOWN)
|
||
btn11 = Pin(11, Pin.IN, Pin.PULL_DOWN)
|
||
btn12 = Pin(12, Pin.IN, Pin.PULL_DOWN)
|
||
time.sleep_ms(20)
|
||
PAIRMODE_ON_START = btn12.value()
|
||
CALIBRATE_ON_START = btn5.value()
|
||
print(PAIRMODE_ON_START)
|
||
adc_3 = ADC(Pin(3))
|
||
adc_4 = ADC(Pin(4))
|
||
adc_8 = ADC(Pin(8))
|
||
adc_9 = ADC(Pin(9))
|
||
adc_1 = ADC(Pin(1))
|
||
adc_10 = ADC(Pin(10))
|
||
|
||
def _espnow_parse_mac(mac):
|
||
if isinstance(mac, bytes):
|
||
return mac
|
||
if isinstance(mac, bytearray):
|
||
return bytes(mac)
|
||
if isinstance(mac, str):
|
||
mac = mac.replace(':', '').replace('-', '').replace(' ', '')
|
||
if len(mac) != 12:
|
||
raise ValueError('MAC must be 12 hex chars')
|
||
return bytes(int(mac[i:i+2], 16) for i in range(0, 12, 2))
|
||
raise ValueError('MAC must be bytes or string')
|
||
|
||
def _espnow_init(channel=6, rate='RATE_LORA_250K'):
|
||
global _espnow_sta, _espnow
|
||
_espnow_sta = network.WLAN(network.STA_IF)
|
||
_espnow_sta.active(True)
|
||
_espnow_sta.disconnect()
|
||
_espnow_sta.config(channel=int(channel), protocol=network.WLAN.PROTOCOL_LR)
|
||
_espnow = espnow.ESPNow()
|
||
_espnow.active(True)
|
||
rate_val = getattr(espnow, str(rate), espnow.RATE_LORA_250K)
|
||
_espnow.config(rate=rate_val)
|
||
return _espnow
|
||
|
||
def _espnow_my_mac():
|
||
if _espnow_sta is None:
|
||
_espnow_init()
|
||
return _espnow_sta.config('mac')
|
||
|
||
def _espnow_add_peer(mac):
|
||
global _espnow_peers
|
||
if _espnow is None:
|
||
_espnow_init()
|
||
peer = _espnow_parse_mac(mac)
|
||
if peer not in _espnow_peers:
|
||
_espnow.add_peer(peer)
|
||
_espnow_peers.append(peer)
|
||
return peer
|
||
|
||
def _espnow_pack(data):
|
||
if isinstance(data, (list, tuple)):
|
||
return ','.join(str(v) for v in data)
|
||
return str(data)
|
||
|
||
def _espnow_send_peer(mac, data):
|
||
if _espnow is None:
|
||
_espnow_init()
|
||
peer = _espnow_add_peer(mac)
|
||
payload = _espnow_pack(data)
|
||
_espnow.send(peer, payload)
|
||
|
||
def _espnow_send_all(data):
|
||
if _espnow is None:
|
||
_espnow_init()
|
||
payload = _espnow_pack(data)
|
||
try:
|
||
_espnow.add_peer(_espnow_broadcast)
|
||
except:
|
||
pass
|
||
_espnow.send(_espnow_broadcast, payload)
|
||
|
||
def _espnow_try_num(s):
|
||
try:
|
||
return int(s)
|
||
except:
|
||
pass
|
||
try:
|
||
return float(s)
|
||
except:
|
||
return s
|
||
|
||
def _espnow_unpack(msg):
|
||
if msg is None:
|
||
return []
|
||
if isinstance(msg, bytes):
|
||
msg = msg.decode('utf-8', 'ignore')
|
||
parts = str(msg).split(',')
|
||
out = []
|
||
for p in parts:
|
||
p = p.strip()
|
||
if p == '':
|
||
continue
|
||
out.append(_espnow_try_num(p))
|
||
return out
|
||
|
||
def _espnow_recv(timeout_ms=10):
|
||
if _espnow is None:
|
||
_espnow_init()
|
||
host, msg = _espnow.recv(timeout_ms)
|
||
return host, _espnow_unpack(msg)
|
||
|
||
# ---------------- Pairing logic ----------------
|
||
PAIR_PREFIX = "PAIR_REQ:"
|
||
PAIR_ACK_PREFIX = "PAIR_ACK:"
|
||
PEER_FILE = "peer_mac.txt"
|
||
peer_mac = None
|
||
pairing_mode = False
|
||
|
||
def load_peer():
|
||
try:
|
||
with open(PEER_FILE, "rb") as f:
|
||
return f.read()
|
||
except:
|
||
return None
|
||
|
||
def save_peer(mac):
|
||
with open(PEER_FILE, "wb") as f:
|
||
f.write(mac)
|
||
|
||
def enter_pairing():
|
||
global pairing_mode, peer_mac
|
||
pairing_mode = True
|
||
my_mac = _espnow_my_mac()
|
||
print("Pairing mode started, broadcasting...")
|
||
while pairing_mode:
|
||
# Broadcast as two fields
|
||
_espnow_send_all([PAIR_PREFIX, my_mac.hex()])
|
||
print("Sent:", [PAIR_PREFIX, my_mac.hex()])
|
||
led.value(not led.value())
|
||
# Non-blocking receive
|
||
host, msg = _espnow_recv(timeout_ms=50)
|
||
if msg and isinstance(msg, list) and len(msg) > 1:
|
||
if msg[0] == PAIR_ACK_PREFIX:
|
||
partner_hex = str(msg[1]).strip()
|
||
if len(partner_hex) == 12: # sanity check
|
||
partner = _espnow_parse_mac(partner_hex)
|
||
save_peer(partner)
|
||
peer_mac = partner
|
||
print("Paired with", partner_hex)
|
||
pairing_mode = False
|
||
time.sleep_ms(500)
|
||
|
||
def calibrate():
|
||
print("CALIBRATING")
|
||
# ADCs
|
||
axes = [adc_3, adc_4, adc_8, adc_9]
|
||
|
||
# --- Centering phase ---
|
||
centers = [0]*len(axes)
|
||
samples = 0
|
||
start = time.ticks_ms()
|
||
next_flash = start
|
||
while time.ticks_diff(time.ticks_ms(), start) < 3000:
|
||
vals = [a.read() for a in axes]
|
||
centers = [c+v for c,v in zip(centers, vals)]
|
||
samples += 1
|
||
# LED flash 6 Hz
|
||
if time.ticks_diff(time.ticks_ms(), next_flash) >= 0:
|
||
led.value(not led.value())
|
||
next_flash = time.ticks_add(next_flash, 83) # ~83ms
|
||
time.sleep_ms(5)
|
||
centers = [int(c/samples) for c in centers]
|
||
print("Centers:", centers)
|
||
|
||
# --- Min/Max phase ---
|
||
mins = [65535]*len(axes)
|
||
maxs = [0]*len(axes)
|
||
start = time.ticks_ms()
|
||
next_flash = start
|
||
while time.ticks_diff(time.ticks_ms(), start) < 5000:
|
||
vals = [a.read() for a in axes]
|
||
mins = [min(m,v) for m,v in zip(mins, vals)]
|
||
maxs = [max(m,v) for m,v in zip(maxs, vals)]
|
||
# LED flash 2 Hz
|
||
if time.ticks_diff(time.ticks_ms(), next_flash) >= 0:
|
||
led.value(not led.value())
|
||
next_flash = time.ticks_add(next_flash, 250) # 250ms
|
||
time.sleep_ms(5)
|
||
print("Min/Max:", list(zip(mins, maxs)))
|
||
|
||
# Return calibration data
|
||
return centers, mins, maxs
|
||
|
||
|
||
def apply_deadzone(norm_val, deadzone=0.05):
|
||
if abs(norm_val) < deadzone:
|
||
return 0.0
|
||
if norm_val > 0:
|
||
return (norm_val - deadzone) / (1.0 - deadzone)
|
||
else:
|
||
return (norm_val + deadzone) / (1.0 - deadzone)
|
||
|
||
def normalize_axis(raw, center, min_val, max_val):
|
||
if raw >= center:
|
||
span = max_val - center
|
||
if span <= 0:
|
||
return 0
|
||
norm = (raw - center) / span
|
||
else:
|
||
span = center - min_val
|
||
if span <= 0:
|
||
return 0
|
||
norm = (raw - center) / span # negative
|
||
# Clamp to [-1, 1]
|
||
if norm > 1:
|
||
norm = 1
|
||
if norm < -1:
|
||
norm = -1
|
||
# Apply deadzone (still in −1…+1)
|
||
norm = apply_deadzone(norm)
|
||
# Scale to −255…+255 and return integer
|
||
return int(norm * 255)
|
||
|
||
|
||
CAL_FILE = "calibration.txt"
|
||
|
||
def save_calibration(centers, mins, maxs):
|
||
with open(CAL_FILE, "w") as f:
|
||
# Write as comma-separated values
|
||
f.write(",".join(str(v) for v in centers) + "\n")
|
||
f.write(",".join(str(v) for v in mins) + "\n")
|
||
f.write(",".join(str(v) for v in maxs) + "\n")
|
||
|
||
def load_calibration():
|
||
try:
|
||
with open(CAL_FILE, "r") as f:
|
||
lines = f.readlines()
|
||
centers = [int(v) for v in lines[0].strip().split(",")]
|
||
mins = [int(v) for v in lines[1].strip().split(",")]
|
||
maxs = [int(v) for v in lines[2].strip().split(",")]
|
||
return centers, mins, maxs
|
||
except:
|
||
return None, None, None
|
||
|
||
# Globals to hold calibration
|
||
cal_centers, cal_mins, cal_maxs = load_calibration()
|
||
if cal_centers:
|
||
print("Loaded calibration:", cal_centers, cal_mins, cal_maxs)
|
||
else:
|
||
print("No calibration stored, will use raw values until calibrated")
|
||
|
||
|
||
|
||
# ---------------- Setup ----------------
|
||
_espnow_init(1, 'RATE_LORA_500K')
|
||
peer_mac = load_peer()
|
||
if not peer_mac:
|
||
enter_pairing()
|
||
else:
|
||
_espnow_add_peer(peer_mac)
|
||
|
||
|
||
|
||
|
||
# ---------------- Main loop ----------------
|
||
while True:
|
||
if btn12.value() == 1 and PAIRMODE_ON_START == 1:
|
||
print("PAIRING")
|
||
start = time.ticks_ms()
|
||
while btn12.value() == 1:
|
||
if time.ticks_diff(time.ticks_ms(), start) > 3000:
|
||
enter_pairing()
|
||
if peer_mac:
|
||
_espnow_add_peer(peer_mac)
|
||
PAIRMODE_ON_START = 0
|
||
break
|
||
else:
|
||
PAIRMODE_ON_START = 0
|
||
|
||
if btn5.value() == 1 and CALIBRATE_ON_START == 1:
|
||
print("CALIBRATING")
|
||
cal_centers, cal_mins, cal_maxs = calibrate()
|
||
print(cal_centers, cal_mins, cal_maxs)
|
||
save_calibration(cal_centers, cal_mins, cal_maxs)
|
||
CALIBRATE_ON_START = 0
|
||
else:
|
||
CALIBRATE_ON_START = 0
|
||
|
||
if peer_mac:
|
||
if cal_centers and cal_mins and cal_maxs:
|
||
# Normalize the four joystick axes
|
||
axes_raw = [adc_3.read(), adc_4.read(), adc_8.read(), adc_9.read()]
|
||
axes_norm = [
|
||
normalize_axis(axes_raw[0], cal_centers[0], cal_mins[0], cal_maxs[0]),
|
||
normalize_axis(axes_raw[1], cal_centers[1], cal_mins[1], cal_maxs[1]),
|
||
normalize_axis(axes_raw[2], cal_centers[2], cal_mins[2], cal_maxs[2]),
|
||
normalize_axis(axes_raw[3], cal_centers[3], cal_mins[3], cal_maxs[3]),
|
||
]
|
||
else:
|
||
# Fallback to raw values if not calibrated
|
||
axes_norm = [adc_3.read(), adc_4.read(), adc_8.read(), adc_9.read()]
|
||
|
||
# Build the packet explicitly
|
||
payload = [
|
||
axes_norm[0], axes_norm[1], axes_norm[2], axes_norm[3],
|
||
adc_1.read(), adc_10.read(),
|
||
btn2.value(), btn5.value(), btn6.value(),
|
||
btn7.value(), btn11.value(), btn12.value()
|
||
]
|
||
_espnow_send_peer(peer_mac, payload)
|
||
led.value(not led.value())
|
||
|
||
time.sleep_ms(50)
|
||
|
||
|