87 lines
2.8 KiB
Python
87 lines
2.8 KiB
Python
# controller.py
|
|
import time
|
|
import struct
|
|
from pathlib import Path
|
|
|
|
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_
|
|
|
|
|
|
def auto_pick_iface() -> str:
|
|
try:
|
|
net_dir = Path("/sys/class/net")
|
|
if not net_dir.exists():
|
|
return "lo"
|
|
names = [p.name for p in net_dir.iterdir() if p.is_dir()]
|
|
for pref in ("en", "eth", "wl"):
|
|
for n in names:
|
|
if n != "lo" and n.startswith(pref):
|
|
return n
|
|
for n in names:
|
|
if n != "lo":
|
|
return n
|
|
return "lo"
|
|
except Exception:
|
|
return "lo"
|
|
|
|
|
|
class unitreeRemoteController:
|
|
def __init__(self):
|
|
self.Lx = 0; self.Rx = 0; self.Ry = 0; self.Ly = 0
|
|
self.L1 = 0; self.L2 = 0; self.R1 = 0; self.R2 = 0
|
|
self.A = 0; self.B = 0; self.X = 0; self.Y = 0
|
|
self.Up = 0; self.Down = 0; self.Left = 0; self.Right = 0
|
|
self.Select = 0; self.F1 = 0; self.F3 = 0; self.Start = 0
|
|
|
|
def parse_botton(self, data1, data2):
|
|
self.R1 = (data1 >> 0) & 1; self.L1 = (data1 >> 1) & 1
|
|
self.Start = (data1 >> 2) & 1; self.Select = (data1 >> 3) & 1
|
|
self.R2 = (data1 >> 4) & 1; self.L2 = (data1 >> 5) & 1
|
|
self.F1 = (data1 >> 6) & 1; self.F3 = (data1 >> 7) & 1
|
|
self.A = (data2 >> 0) & 1; self.B = (data2 >> 1) & 1
|
|
self.X = (data2 >> 2) & 1; self.Y = (data2 >> 3) & 1
|
|
self.Up = (data2 >> 4) & 1; self.Right = (data2 >> 5) & 1
|
|
self.Down = (data2 >> 6) & 1; self.Left = (data2 >> 7) & 1
|
|
|
|
def parse_key(self, data):
|
|
offsets = [4, 8, 12, 20] # Lx, Rx, Ry, Ly
|
|
self.Lx, self.Rx, self.Ry, self.Ly = [struct.unpack('<f', data[o:o+4])[0] for o in offsets]
|
|
|
|
def parse(self, remoteData):
|
|
self.parse_key(remoteData)
|
|
self.parse_botton(remoteData[2], remoteData[3])
|
|
|
|
def get_state(self):
|
|
return self.__dict__.copy()
|
|
|
|
|
|
class LowStateHub:
|
|
def __init__(self, watchdog_timeout=0.25):
|
|
self.low_state = None
|
|
self.first_state = False
|
|
self.last_state_time = 0.0
|
|
self.watchdog_timeout = float(watchdog_timeout)
|
|
|
|
self.remote = unitreeRemoteController()
|
|
self.controller_state = self.remote.get_state()
|
|
|
|
def handler(self, msg: LowState_):
|
|
self.low_state = msg
|
|
self.first_state = True
|
|
self.last_state_time = time.time()
|
|
try:
|
|
self.remote.parse(msg.wireless_remote)
|
|
self.controller_state = self.remote.get_state()
|
|
except Exception:
|
|
pass
|
|
|
|
def fresh(self) -> bool:
|
|
return (time.time() - self.last_state_time) < self.watchdog_timeout
|
|
|
|
def combo_r2x(self) -> bool:
|
|
s = self.controller_state
|
|
return bool(s.get("R2", 0) and s.get("X", 0))
|
|
|
|
def combo_r2l1(self) -> bool:
|
|
s = self.controller_state
|
|
return bool(s.get("R2", 0) and s.get("L1", 0))
|