From 7956b2bef7501d81e7eef65a352d404dca5650b3 Mon Sep 17 00:00:00 2001
From: Makerlab Laptop 1 <laptop_1@makerlab>
Date: Wed, 29 Nov 2023 18:39:23 +0100
Subject: [PATCH] =?UTF-8?q?R=C3=BCckw=C3=A4rsfahren=20mit=20Regler?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 iqrobot.py | 59 ++++++++++++++++++++++++++++++++++++++----------------
 1 file changed, 42 insertions(+), 17 deletions(-)

diff --git a/iqrobot.py b/iqrobot.py
index d2750f3..724a294 100644
--- a/iqrobot.py
+++ b/iqrobot.py
@@ -42,7 +42,6 @@ class IQRobot:
 
         self.abstandsSensor: DistanceSensor = DistanceSensor("D")
 
-
     def show(self, image: str):
         '''
         Zeige Bild auf LED Matrix des Spikes
@@ -50,7 +49,6 @@ class IQRobot:
         '''
         self.hub.light_matrix.show_image(image)
 
-
     def strecke_gefahren(self):
         return -self.linker_motor.get_degrees_counted()/360 * self.rad_umfang
 
@@ -60,12 +58,18 @@ class IQRobot:
 
         :param int grad: Grad um die der Roboter gedreht werden soll
         mittels Vorzeichen +/- kann links oder rechts herum gedreht werden
+        :param bool with_reset: Parameter, um den Gierwinkel zurückzusetzen, Standard: True
         """
+
+        # ist überhaupt etwas zu tun für uns? d.h. grad ist enweder 0 oder 360
         if grad == 0 or grad == 360 :
             print("nichts zu tun")
             return
+
+        # soll der Gierwinkel zurückgesetzt werden?
         if with_reset:
             self.bewegungsSensor.reset_yaw_angle()
+        
         #steering = 100 if grad > 0 else -100
         toleranz = 0
         aktuell = self.bewegungsSensor.get_yaw_angle()
@@ -74,42 +78,65 @@ class IQRobot:
         self.antrieb.start(steering=steering, speed=10)
         differenz = ziel - aktuell
         print ("Start Ziel: {}, Aktuell: {}".format(ziel, aktuell))
+
+        # wiederhole solange der Grad der Drehung noch nicht erreicht ist
         while abs(differenz) > toleranz :
             aktuell = self.bewegungsSensor.get_yaw_angle()
             differenz = ziel - aktuell
             pass
+
+        # stoppe die Bewegung
         self.antrieb.stop()
         print ("Final Ziel: {}, Aktuell: {}".format(ziel, aktuell))
-  
 
     def fahre_gerade_aus(self, cm, speed=20):
+        """
+        Funktion um den Roboter geradeaus fahren zu lassen
+
+        :param int cm: Strecke in cm, die der Roboter geradeaus fahren soll
+        :param int speed: Geschwindigkeit zum Fahren der Strecke, Standard: 20
+        """
+
+        # ist überhaupt etwas zu tun für uns? d.h. cm = 0
+        if cm == 0 :
+            print("nichts zu tun")
+            return
+        
+        # wollen wir vorwärts oder rückwarts fahren?
+        richtung = 1
+        if cm < 0:
+            richtung = -1
+            speed = -speed
+
         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:
+        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
+        versatz = 0                     # aktueller Versatz
+        sum_versatz = 0                 # Summe des Versatzes über Zeit
+
+        # 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 * (1 - abweichung)
-            rechter_speed = speed * (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))
 
         self.antrieb.stop()
         self.drehe(-versatz)
 
-
     def fahre_mit_drehung(self, strecke1, grad, strecke2):
         """
         Funktion für eine Fahrt mit 1. Strecke, dann Drehung in der Mitte, dann 2. Strecke
@@ -119,7 +146,6 @@ class IQRobot:
         self.fahre_gerade_geregelt(strecke1 + self.abstand_rad_front)
         self.drehe(grad)
         self.fahre_gerade_geregelt(strecke2 - self.abstand_rad_front)
-    
 
     def fahre_gerade_aus_alt(self, cm: float, speed: int):
         """
@@ -142,7 +168,7 @@ class IQRobot:
             self.antrieb.move(amount=richtung * 0.1, steering=100)
             drehung = self.bewegungsSensor.get_yaw_angle()
             print(drehung)
-            
+
     def heber(self, cm,speed):
         self.bothFrontMotors.move_tank(-cm*3.3,"cm", -speed, speed)
 
@@ -151,7 +177,6 @@ class IQRobot:
         rotations=volle_umdrehung*prozent/100
         self.bothFrontMotors.move(rotations, unit='rotations',speed=20)
 
-
     def fahre_bis_abstand(self, abstand: int, speed=30, geregelt=True):
         self.antrieb.start_at_power(speed)
         abstand_gerade = self.abstandsSensor.get_distance_cm()