81 lines
2.4 KiB
Python
81 lines
2.4 KiB
Python
import socket
|
|
import struct
|
|
|
|
PACKET_VERSION = 1
|
|
FLOAT_COUNT = 30 * 6 * 3
|
|
|
|
PACKET_FMT = f"<H I B B {FLOAT_COUNT}f"
|
|
PACKET_SIZE = struct.calcsize(PACKET_FMT)
|
|
|
|
import time
|
|
import numpy as np
|
|
from threading import Thread, Event
|
|
from queue import Queue
|
|
|
|
import config as cfg
|
|
import DataTypes as dt
|
|
|
|
|
|
class ESP32Communication(Thread):
|
|
def __init__(self, host=cfg.esp32_ip, port=cfg.esp32_port, timeout=cfg.timeout):
|
|
super().__init__()
|
|
self.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.command_queue = Queue()
|
|
self.response_queue = Queue()
|
|
self.running = Event()
|
|
self.running.set()
|
|
|
|
def run(self):
|
|
while self.running.is_set():
|
|
# 1. Send queued commands
|
|
if not self.command_queue.empty():
|
|
command = self.command_queue.get()
|
|
self._write(command)
|
|
|
|
# 2. Read response
|
|
self.sock.setblocking(False)
|
|
try:
|
|
data = self.sock.recv(1024)
|
|
if data:
|
|
line = data.decode('utf-8', errors='ignore').strip()
|
|
print(f"[ESP32] {line}")
|
|
self.response_queue.put(line)
|
|
except BlockingIOError:
|
|
pass
|
|
|
|
time.sleep(0.01)
|
|
|
|
def write(self, target):
|
|
if isinstance(target, dt.RadArray):
|
|
target = dt.DegArray(np.degrees(target.data))
|
|
leg_str = '\n'.join([','.join(map(str, row)) for row in target])
|
|
formatted_data = f"<{leg_str}>"
|
|
print(f"Gesendete Daten:\n{formatted_data}")
|
|
self._write(formatted_data)
|
|
|
|
def wait4esp32(self, expected_message):
|
|
while True:
|
|
try:
|
|
data = self.sock.recv(1024).decode('utf-8').strip()
|
|
print(f"Empfangene Daten: {data}")
|
|
if data == expected_message:
|
|
print("Bewegung abgeschlossen!")
|
|
break
|
|
except socket.timeout:
|
|
time.sleep(0.05)
|
|
|
|
def _write(self, message):
|
|
self.sock.sendall(message.encode('utf-8'))
|
|
|
|
def stop(self):
|
|
self.running.clear()
|
|
self.join()
|
|
self.sock.close()
|