""" Fahrstategie: Berechnung der Fahrtbefehle für den EasyFollow Autoren: Charlotte Hasenauer, Maximilian Schraysshuen, Nils Landmark Koordinatensystem: Lokales Koordinatensystem im EasyFollow x zeigt nach recht y zeigt nach vorne Geschwindigkeiten: Geschwindigkeiten werden in Prozent angegeben, die Übersetzung in Werte für die Steuerung der Motoren erfolgt auf dem Arduino """ #===================================================================================================== ### SETUP ### # Numpy Modul für Berechnungen import numpy as np # Modul zur Communikation mit dem Arduino import serial # Modul mit Zeitfunktionen import time # Parameter für das Fahren import Parameter as p #===================================================================================================== ### VARIABELN ### # Plot driving route plot_drive_bol = False # Debug Ausgaben debug = False # Variable für die Serial Connection mit dem Arduino ser = None #===================================================================================================== ### FUNKTIONEN/METHODEN ### def cal_dis_and_angle(x: float, y: float) -> tuple[float, float]: """Berechnet aus den x- und y-Koordinaten der Patient*in die Entfernung und den Winkel der Patient*in zum EasyFollow Parameters ---------- x : float x-Koordinate der Patient*in relativ zum EasyFollow. Die x-Richtung zeigt nach "rechts" y : float y-Koordinate der Patient*in relativ zum EasyFollow. Die y-Richtung zeigt nach "vorne" Returns ------- tuple[float, float] (Entfernung, Winkel) Tuple von Entfernung und Winkel """ if debug: print("parse_input") # Entfernung zwischen Patient*in und EasyFollow distance = np.sqrt(x**2 + y**2) if debug: print(f"\t --> distance = {distance}") # Winkel von Patient zu EasyFollow relativ zur y-Achse, Winkel steigt im Uhrzeigersinn # Quadranten 1 and 2 if y > 0: angle = np.arctan(x / y) # Quadrant 4 elif y < 0 and x >= 0: angle = np.arctan(x / y) + np.pi # Quadrant 3 elif y < 0 and x < 0: angle = np.arctan(x / y) - np.pi # Auf y-Achse, x < 0 ("links") elif y == 0 and x > 0: angle = np.pi / 2 # Auf y-Achse, x > 0 ("rechts") elif y == 0 and x < 0: angle = - np.pi / 2 # Im Ursprung elif y == 0 and x == 0: angle = 0 # Bei Fehlerhaften x und y Werten Winkel = 0 else: angle = 0 print(f"fun parse input -> bad position ({x:.2f}, {y:.2f}), distance = {distance}, angle = {angle}") if debug: print(f"\t--> angle = {angle}") return (distance, angle) def cal_turn_speed(angle: float) -> tuple[float, float]: """Berechnung der Drehgeschwindigkeit Parameters ---------- angle : float Winkel der Patient*in zum EasyFollow Returns ------- tuple[float, float] (Geschwindigkeit linkes Rad, Geschwindigkeit rechtes Rad) """ if debug: print("cal_turn") # |Winkel|, Drehgeschwindigkeit hängt nur von Abweichung von 0 ab, nicht vom Vorzeichen angle_abs = abs(angle) # Kopie von Winkelbereiche Liste erstellen, da diese modifiziert wird angle_areas = p.angle_areas.copy() # Winkel in akzeptablen Bereich if angle_abs <= p.angle_area_ok: turn_speed = 0 if debug: print(f"\t--> angle ok, turn_speed = {turn_speed}") # Winkel außerhalb des akzeptablen Bereichs elif angle_abs > p.angle_area_ok: for turn_speed_el in reversed(p.turn_speeds): if angle_abs > (p.angle_area_ok + sum(angle_areas)): turn_speed = turn_speed_el break angle_areas.pop() if debug: print(f"\t--> angle to large, turn_speed = {turn_speed}") else: print(f"Bad angle value: {angle}") turn_speed = 0 # Drehrichtung nach Vorzeichen des Winkels # Linksdrehen if angle < 0: turn = (-turn_speed, turn_speed) if debug: print(f"\t--> turning left, speed = {turn}") # Rechtsdrehen elif angle >= 0: turn = (turn_speed, -turn_speed) if debug: print(f"\t--> turning right, speed = {turn}") return turn def cal_transl_speed(distance: float, angle: float) -> tuple[float, float]: """Berechnung der translatorischen Geschwindigkeit Parameters ---------- distance : float Entfernung angle : float Winkel Returns ------- tuple[float, float] (Geschwindigkeit linkes Rad, Geschwindigkeit rechtes Rad) """ if debug: print("cal_speed") # Kopie der Entfernungsbereiche zu groß Liste, damit sie verändert werden kann close_areas = p.close_areas.copy() # Kopie der Entfernungsbereiche zu klein Liste, damit sie verändert werden kann far_areas = p.far_areas.copy() # |Winkel| >= 90° --> erst Korretur des Winkels, dann translatorische Bewegung if abs(angle) >= np.pi/2: transl_speed = 0 if debug: print(f"\t--> angle is to large, transl_speed = {transl_speed}") # Entfernung in akzeptablem Bereich elif p.distance_min <= distance <= p.distance_max: transl_speed = 0 if debug: print(f"\t--> distance in acceptable range, transl_speed = {transl_speed}") # Enfernung zu klein, Geschwindigkeit abhängig von Bereich in Liste elif distance < p.distance_min: for transl_speed_el in reversed(p.transl_speeds): if distance < (p.distance_min - sum(close_areas)): transl_speed = -transl_speed_el break close_areas.pop() if debug: print(f"\t--> distance is to small, transl_speed = {transl_speed}") # Enfernung zu groß, Geschwindigkeit abhängig von Bereich in Liste elif distance > p.distance_max: for transl_speed_el in reversed(p.transl_speeds): if distance > (p.distance_max + sum(far_areas)): transl_speed = transl_speed_el break far_areas.pop() if debug: print(f"\t--> distance is to large, transl_speed = {transl_speed}") else: print(f"Bad distance value: {distance}") transl_speed = 0 if debug: print(f"\t--> transl_speed = ({transl_speed}, {transl_speed})") return (transl_speed, transl_speed) def parse_ardu(speed: np.array) -> int: """Aus Geschwindigkeitswerten den Befehl für den Arduino berechnen Parameters ---------- speed : np.array Geschwindigkeitswerte für das linke und rechte Rad Returns ------- int 6-Zeichen Int d_l,v_l,v_l,d_r,v_r,v_r 1. Zeichen: Richtung links 2. und 3. Zeichen Geschwindigkeit links 4. Zeichen: Richtung links 5. und 6. Zeichen Geschwindigkeit links Richtung: - 1 = Stopp - 2 = Vorwärts - 3 = Rückwärt Geschwindigkeit: Geschwindigkeit in Prozent - 1 Intervall: [0, 99] Stopp wird über die Richtung kommuniziert und hat Wert 0 (1 Prozent) """ if debug: print("parse_ardu") # Skalierung für Geschwindigkeit > 100% max_abs_speed = max(abs(speed)) if max_abs_speed > 100: speed_corr_faktor = 100 / max_abs_speed speed = np.around(speed * speed_corr_faktor, decimals=0).astype(int) if debug: print(f"corrected speed: {speed}") # Richtung linkes Rad # Stopp if speed[0] == 0: dir_left = 1 # Vorwärts elif speed[0] > 0: dir_left = 2 # Rückwärts elif speed[0] < 0: dir_left = 3 # Geschwindigkeit linkes Rad in Werten [0, 99] speed_left = abs(speed[0]) - 1 # Geschwindigkeit = 0 bei Werten < 0 if speed_left < 0: if debug: print(f"Bad value for left wheel speed: {speed_left}") speed_left = 0 # Richtung linkes Rad # Stopp if speed[1] == 0: dir_right = 1 # Vorwärts elif speed[1] > 0: dir_right = 2 # Rückwärts elif speed[1] < 0: dir_right = 3 # Geschwindigkeit linkes Rad in Werten [0, 99] speed_right = abs(speed[1]) - 1 # Geschwindigkeit = 0 bei Werten < 0 if speed_right < 0: if debug: print(f"Bad value for right wheel speed: {speed_right}") speed_right = 0 # Zusammenbau des 6-Zeichen Fahrbefehls speed_int = 100000 * dir_left + 1000 * speed_left + 100 * dir_right + speed_right if debug: print(f"\t--> Speed number Arduino: {speed_int}") if type(speed_int) != int: if debug: print(f"Bad type for speed_int: {type(speed_int)} {speed_int}") speed_int = int(speed_int) return speed_int # Function to be run on launch ================================================ #Input: x and y coordinates of patient relative to Easy Follow #Output: No variables are returned # Either a 4-digit int to an Arduino through USB and/or a plot for debugging purposes def initialize_serial_connection(port = 'COM3', baudrate = 115200, timeout = .01) -> serial.Serial: """Initialisierung der seriellen Verbindung zum Arduino Parameters ---------- port : str, optional Com Port, an dem der Arduino angeschlossen ist, by default 'COM3' baudrate : int, optional baudrate, by default 115200 timeout : float, optional timeout, by default .01 Returns ------- serial.Serial Serielle Verbindung zum Arduino """ print(f"Establishing serial connection: port={port}, baudrate={baudrate}, timeout={timeout}") # Initialisierung der seriellen Verbindung global ser ser = serial.Serial(port=port, baudrate=baudrate, timeout=timeout) # Warten um dem Arduino die Zeit zum initialisieren zu geben time.sleep(5) return ser def drive(x: float = 1000, y: float = 1000) -> tuple[float, float]: """Fahren: Aufruf zur Berechnung eines Fahrbefehls aus Patien*innenkoordinaten Parameters ---------- x : float, optional x-Koordinaten, by default 1000 y : float, optional y-Koordinaten, by default 1000 Returns ------- tuple[float, float] [description] """ if debug: print("drive") # Abstand und Winkel abstand, winkel = cal_dis_and_angle(x, y) # Drehgeschwindigkeit turn = cal_turn_speed(winkel) # Translatorische Geschwindigkeit translate = cal_transl_speed(abstand, winkel) # Gesammelte Geschwindigkeit speed = np.array(turn) + np.array(translate) # Arduinobefehl speed_int = parse_ardu(speed) print(speed_int) # Sendes des Fahrbefehls an den Arduino ser.write(bytes(str(speed_int), 'utf-8')) return turn, translate