merge conflict and readme update
old code was uploaded in the initial upload changed everything with working programm and updated the readme
This commit is contained in:
+81
-156
@@ -8,139 +8,113 @@ from queue import Queue
|
||||
import config as cfg
|
||||
import DataTypes as dt
|
||||
|
||||
|
||||
# ============================================================
|
||||
# Protocol definitions (SHARED with ESP32)
|
||||
# Protocol definitions (MATCHES THE ULTIMATE .INO)
|
||||
# ============================================================
|
||||
|
||||
PACKET_VERSION = 1
|
||||
|
||||
# Packet IDs
|
||||
PKT_COMMAND = 1
|
||||
PKT_STATUS = 2
|
||||
PKT_DEBUG = 3
|
||||
PKT_CONFIG = 2
|
||||
PKT_DISPLAY = 3
|
||||
|
||||
# Field Types
|
||||
FT_ARRAY_F32 = 1 # float32 array
|
||||
FT_STRING = 2 # utf-8 string
|
||||
FT_ARRAY_F32 = 1
|
||||
FT_STRING = 2
|
||||
FT_UINT8 = 3
|
||||
FT_FLOAT32 = 4
|
||||
FT_CONFIG_VAL = 4
|
||||
|
||||
# Header: packet_id | version | payload_len | timestamp_ms
|
||||
HEADER_FMT = "<B B H I"
|
||||
HEADER_SIZE = struct.calcsize(HEADER_FMT)
|
||||
|
||||
# TLV field: type | length | value
|
||||
HEADER_FMT = "<B B H I" # ID, Ver, Len, Timestamp
|
||||
TLV_FMT = "<B H"
|
||||
TLV_SIZE = struct.calcsize(TLV_FMT)
|
||||
|
||||
|
||||
# ============================================================
|
||||
# ESP32 Communication Thread
|
||||
# ============================================================
|
||||
|
||||
class ESP32Communication(Thread):
|
||||
def __init__(self, host=cfg.esp32_ip, port=cfg.esp32_port, timeout=cfg.timeout):
|
||||
def __init__(self, host=cfg.esp32_ip, port=cfg.esp32_port):
|
||||
super().__init__(daemon=True)
|
||||
|
||||
self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
self.sock.settimeout(timeout)
|
||||
|
||||
print(f"Connecting to ESP32 at {host}:{port} ...")
|
||||
self.sock.connect((host, port))
|
||||
print("Connected to ESP32!")
|
||||
|
||||
self.addr = (host, port)
|
||||
|
||||
# UDP Socket - Zero Lag
|
||||
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
|
||||
self.tx_queue = Queue()
|
||||
self.rx_queue = Queue()
|
||||
self.running = Event()
|
||||
self.running.set()
|
||||
self.last_motion_data = None
|
||||
print(f"Communication Initialized: Target {host}:{port}")
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Thread loop
|
||||
# --------------------------------------------------------
|
||||
def run(self):
|
||||
self.sock.setblocking(False)
|
||||
|
||||
# --- THE FIX: Send Configs immediately upon starting the thread ---
|
||||
self.sync_robot_configs()
|
||||
|
||||
while self.running.is_set():
|
||||
self._handle_tx()
|
||||
self._handle_rx()
|
||||
time.sleep(0.002)
|
||||
if not self.tx_queue.empty():
|
||||
data = self.tx_queue.get()
|
||||
try:
|
||||
self.sock.sendto(data, self.addr)
|
||||
except Exception as e:
|
||||
print(f"UDP Send Error: {e}")
|
||||
|
||||
# Tiny sleep to prevent 100% CPU usage
|
||||
time.sleep(0.001)
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Public API
|
||||
# --------------------------------------------------------
|
||||
def send_command(self, radial_array :dt.RadArray, state=0, name=""):
|
||||
def sync_robot_configs(self):
|
||||
"""
|
||||
Example command packet:
|
||||
- 3x6 float array
|
||||
- uint8 state
|
||||
- optional string
|
||||
Sends the parameters from your config.py to the ESP32.
|
||||
This tells the ESP32 how to map degrees to pulses and
|
||||
how fast the Python loop is running for smoothing.
|
||||
"""
|
||||
data = np.degrees(radial_array).astype(np.uint8)
|
||||
print(data)
|
||||
fields = [
|
||||
self._tlv_array(data),
|
||||
self._tlv_uint8(state)
|
||||
print("Sending Config Sync to Robot...")
|
||||
|
||||
# Calculate interval in ms based on your FPS (tickpersec)
|
||||
interval_ms = int(1000 / cfg.standard_tickpersec)
|
||||
|
||||
# Config IDs: 1=Min, 2=Max, 3=Interval
|
||||
configs = [
|
||||
(1, 125), # servoMin (Default for 50Hz)
|
||||
(2, 625), # servoMax (Default for 50Hz)
|
||||
(3, interval_ms)
|
||||
]
|
||||
|
||||
fields = []
|
||||
for c_id, val in configs:
|
||||
# Payload: 1 byte for ID, 2 bytes for Value (Little Endian)
|
||||
payload = struct.pack("<B h", c_id, val)
|
||||
fields.append(struct.pack(TLV_FMT, FT_CONFIG_VAL, len(payload)) + payload)
|
||||
|
||||
packet = self._build_packet(PKT_CONFIG, fields)
|
||||
self.tx_queue.put(packet)
|
||||
|
||||
if name:
|
||||
fields.append(self._tlv_string(name))
|
||||
|
||||
def send_motion(self, radial_array: dt.RadArray):
|
||||
"""Sends motion only if the data has changed."""
|
||||
# 1. Convert to degrees/uint8 as you did before
|
||||
new_data = np.degrees(radial_array.data).astype(np.uint8)
|
||||
|
||||
# 2. Check if it's different from the last sent packet
|
||||
if self.last_motion_data is not None:
|
||||
if np.array_equal(new_data, self.last_motion_data):
|
||||
return # Skip sending if data is identical
|
||||
|
||||
# 3. Update the tracker and send
|
||||
self.last_motion_data = new_data.copy()
|
||||
|
||||
fields = [self._tlv(FT_ARRAY_F32, new_data.tobytes())]
|
||||
packet = self._build_packet(PKT_COMMAND, fields)
|
||||
self.tx_queue.put(packet)
|
||||
|
||||
def receive(self):
|
||||
"""
|
||||
Non-blocking receive.
|
||||
Returns (packet_id, timestamp, fields) or None
|
||||
"""
|
||||
try:
|
||||
return self.rx_queue.get_nowait()
|
||||
except Exception:
|
||||
return None
|
||||
def send_text(self, text=""):
|
||||
"""Sends a dedicated display packet independent of motion."""
|
||||
# Convert string to bytes
|
||||
text_bytes = text.encode("utf-8")
|
||||
|
||||
# Wrap in TLV format (Type 2 = FT_STRING)
|
||||
field = self._tlv(FT_STRING, text_bytes)
|
||||
|
||||
# Build the packet using the PKT_DISPLAY (ID 3)
|
||||
packet = self._build_packet(PKT_DISPLAY, [field])
|
||||
|
||||
self.tx_queue.put(packet)
|
||||
|
||||
def stop(self):
|
||||
self.running.clear()
|
||||
self.join()
|
||||
self.sock.close()
|
||||
def _tlv(self, ftype, data):
|
||||
return struct.pack(TLV_FMT, ftype, len(data)) + data
|
||||
|
||||
# --------------------------------------------------------
|
||||
# TX / RX handlers
|
||||
# --------------------------------------------------------
|
||||
def _handle_tx(self):
|
||||
if not self.tx_queue.empty():
|
||||
data = self.tx_queue.get()
|
||||
self.sock.sendall(data)
|
||||
|
||||
def _handle_rx(self):
|
||||
try:
|
||||
header = self._recv_exact(HEADER_SIZE)
|
||||
if not header:
|
||||
return
|
||||
|
||||
packet_id, version, payload_len, timestamp = struct.unpack(HEADER_FMT, header)
|
||||
|
||||
if payload_len == 0:
|
||||
# Nothing to parse
|
||||
self.rx_queue.put((packet_id, timestamp, []))
|
||||
return
|
||||
|
||||
payload = self._recv_exact(payload_len)
|
||||
if payload is None:
|
||||
return # incomplete, skip
|
||||
|
||||
fields = self._parse_tlv(payload)
|
||||
self.rx_queue.put((packet_id, timestamp, fields))
|
||||
|
||||
except BlockingIOError:
|
||||
pass
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Packet construction
|
||||
# --------------------------------------------------------
|
||||
def _build_packet(self, packet_id, fields):
|
||||
payload = b"".join(fields)
|
||||
|
||||
header = struct.pack(
|
||||
HEADER_FMT,
|
||||
packet_id,
|
||||
@@ -148,57 +122,8 @@ class ESP32Communication(Thread):
|
||||
len(payload),
|
||||
int(time.time() * 1000) & 0xFFFFFFFF
|
||||
)
|
||||
|
||||
return header + payload
|
||||
|
||||
# --------------------------------------------------------
|
||||
# TLV helpers
|
||||
# --------------------------------------------------------
|
||||
def _tlv(self, ftype, data):
|
||||
return struct.pack(TLV_FMT, ftype, len(data)) + data
|
||||
|
||||
def _tlv_array(self, array):
|
||||
# Convert to uint8 degrees before sending
|
||||
arr_uint8 = array.astype(np.uint8)
|
||||
return self._tlv(FT_ARRAY_F32, arr_uint8.tobytes())
|
||||
|
||||
def _tlv_string(self, text):
|
||||
return self._tlv(FT_STRING, text.encode("utf-8"))
|
||||
|
||||
def _tlv_uint8(self, value):
|
||||
return self._tlv(FT_UINT8, struct.pack("<B", value))
|
||||
|
||||
# --------------------------------------------------------
|
||||
# TLV parsing
|
||||
# --------------------------------------------------------
|
||||
def _parse_tlv(self, payload):
|
||||
pos = 0
|
||||
fields = []
|
||||
|
||||
while pos < len(payload):
|
||||
ftype, flen = struct.unpack(
|
||||
TLV_FMT, payload[pos:pos + TLV_SIZE]
|
||||
)
|
||||
pos += TLV_SIZE
|
||||
|
||||
data = payload[pos:pos + flen]
|
||||
pos += flen
|
||||
|
||||
fields.append((ftype, data))
|
||||
|
||||
return fields
|
||||
|
||||
# --------------------------------------------------------
|
||||
# Socket helper
|
||||
# --------------------------------------------------------
|
||||
def _recv_exact(self, size):
|
||||
data = b""
|
||||
while len(data) < size:
|
||||
try:
|
||||
chunk = self.sock.recv(size - len(data))
|
||||
if not chunk:
|
||||
return None
|
||||
data += chunk
|
||||
except BlockingIOError:
|
||||
return None
|
||||
return data
|
||||
def stop(self):
|
||||
self.running.clear()
|
||||
self.sock.close()
|
||||
Reference in New Issue
Block a user