84 lines
2.9 KiB
Python
84 lines
2.9 KiB
Python
from threading import Thread, Event
|
|
from queue import Queue
|
|
import numpy as np
|
|
import serial
|
|
import time
|
|
|
|
from config import cfg
|
|
import DataTypes as dt
|
|
|
|
class ArduinoCommunication(Thread):
|
|
def __init__(self, port=cfg.port, baudrate=cfg.baudrate, timeout=cfg.comm_timeout):
|
|
super().__init__()
|
|
self.daemon = True # Thread schlie�t sich beim Programmende
|
|
self.serial_conn = serial.Serial(port, baudrate, timeout=timeout)
|
|
time.sleep(2) # Warten bis Arduino ready
|
|
|
|
self.command_queue = Queue()
|
|
self.response_queue = Queue()
|
|
self.running = Event()
|
|
self.running.set()
|
|
|
|
def send_motion(self, radial_array):
|
|
self.write(radial_array)
|
|
|
|
def run(self):
|
|
while self.running.is_set():
|
|
# 1. Befehle senden
|
|
if not self.command_queue.empty():
|
|
command = self.command_queue.get()
|
|
self._write(command)
|
|
|
|
# 2. Antwort lesen (falls vorhanden)
|
|
if self.serial_conn.in_waiting > 0:
|
|
line = self.serial_conn.readline().decode('utf-8', errors='ignore').strip()
|
|
if line:
|
|
print(f"[Arduino] {line}")
|
|
self.response_queue.put(line)
|
|
|
|
time.sleep(0.01)
|
|
|
|
# Werte auf Arduino schreiben
|
|
def write(self, target):
|
|
if isinstance(target, dt.RadArray):
|
|
target = dt.DegArray(np.degrees(target.data))
|
|
if self.serial_conn and self.serial_conn.is_open:
|
|
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.serial_conn.write(bytes(formatted_data, 'utf-8'))
|
|
|
|
# Werte von Arduino ablesen und Synchronisierunga
|
|
def wait4arduino(self, expected_message):
|
|
while True:
|
|
# Lese eine Zeile von der seriellen Verbindung
|
|
data = self.serial_conn .readline()
|
|
|
|
# Konvertiere die Zeile in einen lesbaren String
|
|
data = str(data, "utf-8").strip("\r\n")
|
|
|
|
# Debugging-Ausgabe (optional)
|
|
print(f"Empfangene Daten: {data}")
|
|
|
|
# Ueberpruefe, ob die empfangene Nachricht der erwarteten Nachricht entspricht
|
|
if data == expected_message:
|
|
print("Bewegung abgeschlossen!")
|
|
break
|
|
else:
|
|
time.sleep(0.05)
|
|
|
|
def _write(self, message):
|
|
if self.serial_conn and self.serial_conn.is_open:
|
|
self.serial_conn.write(message.encode('utf-8'))
|
|
|
|
def stop(self):
|
|
self.running.clear()
|
|
try:
|
|
self.join(timeout=0.5)
|
|
except RuntimeError:
|
|
pass
|
|
if self.serial_conn and self.serial_conn.is_open:
|
|
self.serial_conn.close()
|
|
|
|
def close(self):
|
|
self.stop() |