README.md

@@ -16,7 +16,7 @@ Importiere den Code entweder über die Shell
 oder einen Git Client:
 - für Mac oder Windows: https://www.sourcetreeapp.com/ 
 
-Username und PW für Makerlab eingeben
+Benutzername und Passwort für Makerlab eingeben
 
 Übersicht über Git Commands: https://ndpsoftware.com/git-cheatsheet.html#loc=workspace;
 

iqrobot.py

@@ -2,7 +2,7 @@
 
 import math
 
-from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor
+from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor, DistanceSensor
 from spike.control import wait_for_seconds
 
 HELLO = "HELLO IQ"
@@ -27,17 +27,21 @@ 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()
 
+        self.abstandsSensor: DistanceSensor = DistanceSensor("D")
+
 
     def show(self, image: str):
         '''
@@ -47,6 +51,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 +82,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 +121,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
 
@@ -135,6 +151,15 @@ 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()
+        while abstand_gerade > abstand:
+            abstand_gerade = self.abstandsSensor.get_distance_cm()
+            print(str(abstand_gerade))
+        self.antrieb.stop()
+
         
 print("successfully loaded the IQ Lego teams code :)")
 
@@ -145,3 +170,4 @@ print("successfully loaded the IQ Lego teams code :)")
 
 
 
+

main.py

@@ -79,6 +79,14 @@ iqRobot: iq.IQRobot = iq.IQRobot(hub)
 # Führe Funktionen aus unser Robot Klasse aus:
 iqRobot.show('HAPPY')
 
+def huenchenaufgabe():
+    iqRobot.fahre_gerade_aus(40,60)
+    iqRobot.drehe(-40,True)
+    iqRobot.fahre_gerade_aus(20,60)
+    iqRobot.drehe(-20)
+    iqRobot.fahre_gerade_aus(55,60)
+    iqRobot.heber(10,30)
+
 def hologram_alt():
     iqRobot.fahre_gerade_aus(cm=75,speed=80)
     iqRobot.drehe(45, False)
@@ -87,7 +95,6 @@ def hologram_alt():
     iqRobot.drehe(-45, False)
     iqRobot.fahre_gerade_aus(cm=-75,speed=50)
 
-
 def druckmaschine():
     iqRobot.fahre_gerade_aus(19,30)
     iqRobot.drehe(-45)  
@@ -116,4 +123,4 @@ def augmented_reality():
 
 druckmaschine()
 hologram()
-augmented_reality()
+augmented_reality()