From 8304ac28b675b7b6fd4e548b0c3e204b656d9b47 Mon Sep 17 00:00:00 2001 From: Makerlab Laptop 1 Date: Wed, 29 Nov 2023 18:57:07 +0100 Subject: [PATCH] t --- iqrobot.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/iqrobot.py b/iqrobot.py index eba783c..724a294 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -106,6 +106,7 @@ class IQRobot: richtung = 1 if cm < 0: richtung = -1 + speed = -speed self.linker_motor.set_degrees_counted(0) self.bewegungsSensor.reset_yaw_angle() @@ -113,8 +114,8 @@ class IQRobot: self.antrieb.start_tank(10, 10) self.antrieb.set_default_speed(10) - linker_speed=speed * richtung # Geschwindigkeit linker Motor - rechter_speed=speed * richtung # Geschwindigkeit rechter Motor + linker_speed=speed # Geschwindigkeit linker Motor + rechter_speed=speed # Geschwindigkeit rechter Motor kp = 1.5 # Verstärkungsfaktor zur Regelung ki = 1.0 # Integralfaktor zur Regelung sum_cm = 0 # bereits gefahrene Strecke @@ -124,12 +125,12 @@ class IQRobot: # wiederhole solange die gefahrene Strecke noch nicht erreicht ist while sum_cm < cm * richtung: wait_for_seconds(0.05) - sum_cm = self.strecke_gefahren() + sum_cm = self.strecke_gefahren() * richtung versatz = self.bewegungsSensor.get_yaw_angle() sum_versatz = sum_versatz + versatz abweichung = (kp * versatz + ki * sum_versatz) / 100 - linker_speed = speed * richtung * (1 - abweichung) - rechter_speed = speed * richtung * (1 + abweichung) + linker_speed = speed * (1 - abweichung * richtung) + rechter_speed = speed * (1 + abweichung * richtung) self.antrieb.start_tank_at_power(int(linker_speed), int(rechter_speed)) #print("Versatz: " + str(versatz) + " , linker Speed: " + str(linker_speed) + ", rechter Speed: " + str(rechter_speed) + ", strecke: " + str(sum_cm))