Merge pull request 'Rückwärtsfahren mit Regler' (#16) from fixes into main

Reviewed-on: #16
Reviewed-by: Vincenz Ernst <viernst@linuxmuster.lan>

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))