JackM323
2b2125bfde
old code was uploaded in the initial upload changed everything with working programm and updated the readme
129 lines
4.3 KiB
Python
129 lines
4.3 KiB
Python
import socket
|
|
import struct
|
|
import time
|
|
import numpy as np
|
|
from threading import Thread, Event
|
|
from queue import Queue
|
|
|
|
import config as cfg
|
|
import DataTypes as dt
|
|
|
|
# ============================================================
|
|
# Protocol definitions (MATCHES THE ULTIMATE .INO)
|
|
# ============================================================
|
|
PACKET_VERSION = 1
|
|
PKT_COMMAND = 1
|
|
PKT_CONFIG = 2
|
|
PKT_DISPLAY = 3
|
|
|
|
FT_ARRAY_F32 = 1
|
|
FT_STRING = 2
|
|
FT_UINT8 = 3
|
|
FT_CONFIG_VAL = 4
|
|
|
|
HEADER_FMT = "<B B H I" # ID, Ver, Len, Timestamp
|
|
TLV_FMT = "<B H"
|
|
|
|
class ESP32Communication(Thread):
|
|
def __init__(self, host=cfg.esp32_ip, port=cfg.esp32_port):
|
|
super().__init__(daemon=True)
|
|
self.addr = (host, port)
|
|
|
|
# UDP Socket - Zero Lag
|
|
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
|
|
self.tx_queue = Queue()
|
|
self.running = Event()
|
|
self.running.set()
|
|
self.last_motion_data = None
|
|
print(f"Communication Initialized: Target {host}:{port}")
|
|
|
|
def run(self):
|
|
# --- THE FIX: Send Configs immediately upon starting the thread ---
|
|
self.sync_robot_configs()
|
|
|
|
while self.running.is_set():
|
|
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)
|
|
|
|
def sync_robot_configs(self):
|
|
"""
|
|
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.
|
|
"""
|
|
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)
|
|
|
|
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 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 _tlv(self, ftype, data):
|
|
return struct.pack(TLV_FMT, ftype, len(data)) + data
|
|
|
|
def _build_packet(self, packet_id, fields):
|
|
payload = b"".join(fields)
|
|
header = struct.pack(
|
|
HEADER_FMT,
|
|
packet_id,
|
|
PACKET_VERSION,
|
|
len(payload),
|
|
int(time.time() * 1000) & 0xFFFFFFFF
|
|
)
|
|
return header + payload
|
|
|
|
def stop(self):
|
|
self.running.clear()
|
|
self.sock.close() |