From cd47da6f23b60ae4665553185d64e8d883fba635 Mon Sep 17 00:00:00 2001
From: Lars Haferkamp <>
Date: Wed, 22 Nov 2023 18:56:19 +0100
Subject: [PATCH] Gerade aus Fahr Funktion mit PI Regler

---
 iqrobot.py | 38 ++++++++++++++++++++++++++------------
 1 file changed, 26 insertions(+), 12 deletions(-)

diff --git a/iqrobot.py b/iqrobot.py
index 0ea4b35..cec7049 100644
--- a/iqrobot.py
+++ b/iqrobot.py
@@ -27,14 +27,16 @@ class IQRobot:
 
         self.bothFrontMotors: MotorPair = MotorPair(FRONT_MOTOR_LEFT_PORT, FRONT_MOTOR_RIGHT_PORT)
 
+        self.linker_motor: Motor = Motor(LEFT_MOTOR_PORT)
+        
         self.antrieb: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT)
         # Radumfang neu berechnen und Motor konfigurieren
         # Radius der Antriebsräder
         self.rad_radius = 2.1
         # Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters
         self.abstand_rad_front = 5.55            
-        rad_umfang = 2 * math.pi * self.rad_radius
-        self.antrieb.set_motor_rotation(rad_umfang)
+        self.rad_umfang = 2 * math.pi * self.rad_radius
+        self.antrieb.set_motor_rotation(self.rad_umfang)
 
         self.bewegungsSensor: MotionSensor = MotionSensor()
 
@@ -47,6 +49,9 @@ class IQRobot:
         self.hub.light_matrix.show_image(image)
 
 
+    def strecke_gefahren(self):
+        return -self.linker_motor.get_degrees_counted()/360 * self.rad_umfang
+
     def drehe(self, grad=90, with_reset=True):
         """
         Funktion um den Roboter auf der Stelle zu drehen
@@ -75,23 +80,32 @@ class IQRobot:
         print ("Final Ziel: {}, Aktuell: {}".format(ziel, aktuell))
   
 
-    def fahre_gerade_geregelt(self, cm):
-        """
-        WIP by Lars & Klaus
-        """
+    def fahre_gerade_aus(self, cm, speed=20):
+        self.linker_motor.set_degrees_counted(0)
         self.bewegungsSensor.reset_yaw_angle()
+
         self.antrieb.start_tank(10, 10)
         self.antrieb.set_default_speed(10)
 
+        linker_speed=speed
+        rechter_speed=speed
+        kp = 1.5
+        ki = 1.0
         sum_cm = 0
+        sum_versatz = 0
         while sum_cm < cm:
-            self.antrieb.move(1)
+            wait_for_seconds(0.05)
+            sum_cm = self.strecke_gefahren()
             versatz = self.bewegungsSensor.get_yaw_angle()
-            self.drehe(grad=-versatz)
-            self.bewegungsSensor.reset_yaw_angle()
-            sum_cm = sum_cm + 1
+            sum_versatz = sum_versatz + versatz
+            abweichung = (kp * versatz + ki * sum_versatz) / 100
+            linker_speed = speed * (1 - abweichung)
+            rechter_speed = speed * (1 + abweichung)
+            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))
 
-        self.antrieb.move(cm - sum_cm)
+        self.antrieb.stop()
+        self.drehe(-versatz)
 
 
     def fahre_mit_drehung(self, strecke1, grad, strecke2):
@@ -105,7 +119,7 @@ class IQRobot:
         self.fahre_gerade_geregelt(strecke2 - self.abstand_rad_front)
     
 
-    def fahre_gerade_aus(self, cm: float, speed: int):
+    def fahre_gerade_aus_alt(self, cm: float, speed: int):
         """
         Funktion zum gerade aus fahren mit Korrektur am Ende