Merge pull request 'Funktion für neue Schaufel und Doku' (#19) from laptop02 into main

Reviewed-on: #19

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,13 +2,11 @@
 
 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"
-BRICKIES_BOT = "brickies"
-BRICKIES_BOT_2 = "brickies_2"
-BACKSTEIN_BOT = "backstein"
+print("Lade IQ-Bibliothek")
+
 
 '''
 Wir nutzen "Duck typing", dh wir schreiben hinter jede Variabel mit ':' die Klasse, zB `leftMotor: Motor`
@@ -16,54 +14,38 @@ damit man dann später auch wieder Code Completion hat bei Nutzung der Variablen
 '''
 class IQRobot:
 
-    def __init__(self, hub: PrimeHub, typ: str):
+    def __init__(self, hub: PrimeHub):
         
         self.hub: PrimeHub = hub
-        self.typ=typ
 
-        if self.typ==BACKSTEIN_BOT:
-            # Radantrieb
-            LEFT_MOTOR_PORT = 'F'
-            RIGHT_MOTOR_PORT = 'B'
-            # Motoren für Aufsätze
-            FRONT_MOTOR_RIGHT_PORT = "E"
-            self.frontMotorRight: Motor = Motor(FRONT_MOTOR_RIGHT_PORT)
+        # Radantrieb
+        LEFT_MOTOR_PORT = 'E'
+        RIGHT_MOTOR_PORT = 'F'
 
-        elif self.typ==BRICKIES_BOT:
-            # Radantrieb
-            LEFT_MOTOR_PORT = 'E'
-            RIGHT_MOTOR_PORT = 'F'
-            # Motoren für Aufsätze
-            FRONT_MOTOR_RIGHT_PORT = "B"
-            FRONT_MOTOR_LEFT_PORT = "A"
-            self.frontMotorRight: Motor = Motor(FRONT_MOTOR_RIGHT_PORT)
-            self.frontMotorLeft: Motor = Motor(FRONT_MOTOR_LEFT_PORT)
-            self.bothFrontMotors: MotorPair = MotorPair(FRONT_MOTOR_LEFT_PORT, FRONT_MOTOR_RIGHT_PORT)
-            # Radius der Antriebsräder
-            self.rad_radius = 2.1
-            # Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters
-            self.abstand_rad_front = 5.55
-            
-        elif self.typ==BRICKIES_BOT_2:    
-            # Radantrieb
-            LEFT_MOTOR_PORT = 'E'
-            RIGHT_MOTOR_PORT = 'F'
-            # Radius der Antriebsräder
-            self.rad_radius = 2.9
-            # Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters
-            self.abstand_rad_front = 8.5
-            
-        ## Allgemein ##        
-        self.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT)
+        # Motoren für Aufsätze
+        FRONT_MOTOR_RIGHT_PORT = "B"
+        FRONT_MOTOR_LEFT_PORT = "A"
+
+        self.bothFrontMotors: MotorPair = MotorPair(FRONT_MOTOR_LEFT_PORT, FRONT_MOTOR_RIGHT_PORT)
+        self.linker_motor_vorne: Motor = Motor(FRONT_MOTOR_LEFT_PORT)
+        self.linker_motor_vorne.set_stall_detection(stop_when_stalled=True)
+        self.rechter_motor_vorne: Motor = Motor(FRONT_MOTOR_RIGHT_PORT)
+        self.rechter_motor_vorne.set_stall_detection(stop_when_stalled=True)
+
+        self.linker_motor: Motor = Motor(LEFT_MOTOR_PORT)
+        
+        self.antrieb: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT)
         # Radumfang neu berechnen und Motor konfigurieren
-        rad_umfang = 2 * math.pi * self.rad_radius
-        self.movementMotors.set_motor_rotation(rad_umfang)
-        self.leftMotor: Motor = Motor(LEFT_MOTOR_PORT)
-        self.rightMotor: Motor = Motor(RIGHT_MOTOR_PORT)         
-        #self.colorSensor: ColorSensor = ColorSensor(colorSensorPort)
-        #self.frontMotorLeft: Motor = Motor("C")
-        self.motionSensor: MotionSensor = MotionSensor()
+        # Radius der Antriebsräder
+        self.rad_radius = 2.1
+        # Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters
+        self.abstand_rad_front = 5.55            
+        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):
         '''
@@ -72,93 +54,175 @@ class IQRobot:
         '''
         self.hub.light_matrix.show_image(image)
 
-
-    def driveForward_for_sec(self, seconds: float):
-        # Fahre die übergebene Anzahl seconds gerade aus
-        self.movementMotors.start()
-        wait_for_seconds(seconds)
-        self.movementMotors.stop()
+    def strecke_gefahren(self):
+        '''
+        Gibt die gefahrene Strecke basierend auf den Radumdrehungen zurück 
+        '''
+        return -self.linker_motor.get_degrees_counted()/360 * self.rad_umfang
 
 
-    def getColorIntensity(self):
-        # Ermittele Farbintensität über den Farbsensor
-        (red, green, blue, colorIntensity) = self   .colorSensor.get_rgb_intensity()
-        return colorIntensity
+    def drehe(self, grad=90, with_reset=True, speed=10):
+        """
+        Funktion um den Roboter auf der Stelle zu drehen
 
+        :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
+        """
 
-    def drehe(self, grad=90, with_reset=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.motionSensor.reset_yaw_angle()
+            self.bewegungsSensor.reset_yaw_angle() # Gierwinkel zurücksetzen
+        
         #steering = 100 if grad > 0 else -100
-        toleranz = 0
-        aktuell = self.motionSensor.get_yaw_angle()
+        toleranz = 0 # Toleranz soll null sein. Kann erhöht werden, falls der Roboter sich unendlich dreht.
+        aktuell = self.bewegungsSensor.get_yaw_angle() # Aktuelle Position
         ziel = grad
         steering = 100 if ziel > aktuell else -100
-        self.movementMotors.start(steering=steering, speed=10)
+        self.antrieb.start(steering=steering, speed=speed) # Mit bestimmer Geschwindigkeit starten
         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.motionSensor.get_yaw_angle()
+            aktuell = self.bewegungsSensor.get_yaw_angle()
             differenz = ziel - aktuell
             pass
-        self.movementMotors.stop()
+
+        # 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
 
-    def fahre_gerade(self, cm):
-        if self.typ == "brickies":
-            cm = -cm
-        self.motionSensor.reset_yaw_angle()
-        self.movementMotors.start_tank(10, 10)
-        self.movementMotors.set_default_speed(10)
-        self.movementMotors.move(cm)
-        versatz = self.motionSensor.get_yaw_angle()
-        self.drehe(grad=-versatz)
-    
+        :param int cm: Strecke in cm, die der Roboter geradeaus fahren soll
+        :param int speed: Geschwindigkeit zum Fahren der Strecke, Standard: 20
+        """
 
-    def fahre_gerade_geregelt(self, cm):
-        if self.typ == "brickies":
-            cm = -cm
-        self.motionSensor.reset_yaw_angle()
-        self.movementMotors.start_tank(10, 10)
-        self.movementMotors.set_default_speed(10)
+        # 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 * richtung # Die Geschwindigkeit soll negativ sein, wenn wir rückwärts fahren
 
-        sum_cm = 0
-        while sum_cm < cm:
-            self.movementMotors.move(1)
-            versatz = self.motionSensor.get_yaw_angle()
-            self.drehe(grad=-versatz)
-            self.motionSensor.reset_yaw_angle()
-            sum_cm = sum_cm + 1
+        # Alles zurücksetzen
+        self.linker_motor.set_degrees_counted(0) 
+        self.bewegungsSensor.reset_yaw_angle()
 
-        self.movementMotors.move(cm - sum_cm)
+        # Mit irgendeiner Geschwindigkeit g>0 starten. Wert ist irrelevant
+        self.antrieb.start_tank(10, 10)
+        self.antrieb.set_default_speed(10)
+
+        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) # Sonst wird das zu oft ausgeführt
+            sum_cm = self.strecke_gefahren() * richtung # Gefahrene Strecke, ggf. eben negativ machen
+            versatz = self.bewegungsSensor.get_yaw_angle() # Um wie viel sind wir falsch?
+            sum_versatz = sum_versatz + versatz
+            abweichung = (kp * versatz + ki * sum_versatz) / 100 # Abweichung berechnen, um zu korrigieren
+            linker_speed = speed * (1 - abweichung * richtung) 
+            rechter_speed = speed * (1 + abweichung * richtung)
+            self.antrieb.start_tank_at_power(int(linker_speed), int(rechter_speed)) # Mit neuer Geschwindigkeit starten
+            #print("Versatz: " + str(versatz) + " , linker Speed: " + str(linker_speed) + ", rechter Speed: " + str(rechter_speed) + ", strecke: " + str(sum_cm))
+
+        self.antrieb.stop() # Stoppen
+        self.drehe(-versatz) # Da Versatz immer != 0, korrigieren
 
     def fahre_mit_drehung(self, strecke1, grad, strecke2):
+        """
+        Funktion für eine Fahrt mit 1. Strecke, dann Drehung in der Mitte, dann 2. Strecke
+        Vereinfacht die Logik, da der Roboter durch die Drehung einen Versatz hat gegenüber einer
+        Strecke die mit dem Lineal ausgemessen wurde
+        """
         self.fahre_gerade_geregelt(strecke1 + self.abstand_rad_front)
         self.drehe(grad)
         self.fahre_gerade_geregelt(strecke2 - self.abstand_rad_front)
+
+
+    #deprecated
+    def fahre_gerade_aus_alt(self, cm: float, speed: int):
+        """
+        Funktion zum gerade aus fahren mit Korrektur am Ende
+        Wird nicht mehr aktiv genutzt, da wir jetzt fahre_gerade_aus haben, 
+        welche geregelt ist, und der Roboter daher nicht schief wird.
+
+        :param int cm: Zentimeter die gerade aus gefahren werden soll
+        :param speed: Geschwindigkeit mit der gefahren wird
+        """
     
-    def fahre_gerade_aus(self, cm,speed):
+        self.bewegungsSensor.reset_yaw_angle()
     
-        self.motionSensor.reset_yaw_angle()
-    
-        self.movementMotors.move_tank(amount=cm,left_speed=speed, right_speed=speed)
-        drehung = self.motionSensor.get_yaw_angle()
+        self.antrieb.move_tank(amount=cm,left_speed=speed, right_speed=speed)
+        drehung = self.bewegungsSensor.get_yaw_angle()
         print(drehung)
         if drehung > 0:
             richtung = -1
         else:
             richtung = 1
         while abs(drehung) > 2:
-            self.movementMotors.move(amount=richtung * 0.1, steering=100)
-            drehung = self.motionSensor.get_yaw_angle()
+            self.antrieb.move(amount=richtung * 0.1, steering=100)
+            drehung = self.bewegungsSensor.get_yaw_angle()
             print(drehung)
 
+    def heber(self, cm,speed):
+        """
+        Lässt den Heber fahren
+
+        :param speed: Geschwindigkeit, mit der der Heber bewegt wird
+        :param cm: Um wie viel soll der Heber bewegt werden?
+        """
+        self.bothFrontMotors.move_tank(-cm*3.3,"cm", -speed, speed) # Heber bewegen
+
+    def schaufel(self,prozent, speed=20):
+        """
+        Lässt die Schaufel fahren
+
+        :param prozent: Auf wie viel Prozent soll die Schaufel bewegt werden?
+        """
+        volle_umdrehung=0.29
+        rotations=volle_umdrehung*prozent/100
+        #self.bothFrontMotors.move(rotations, unit='rotations',speed=20)
+        self.bothFrontMotors.move_tank(rotations, 'rotations', speed, -speed)
+
+    # TODO: Geregeltes Fahren ist noch nicht eingebaut
+    def fahre_bis_abstand(self, abstand: int, speed=30, geregelt=True):
+        """
+        Nutzt den Abstandssensor, um zu fahren, bis ein Abstand erreicht ist
+
+        :param abstand: Abstand zum Objekt
+        :param speed: Geschwindigkeit, mit der gefahren wird
+        :param geregelt: Soll mit Regler gefahren werden?
+        """
+        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 :)")
+print("Fertig geladen.")
+
 
 
 

main.py

@@ -27,11 +27,11 @@ def importFile(slotid=0, precompiled=False, module_name='importFile'):
     with open("/projects/.slots","rt") as f:
         slots = eval(str(f.read()))
         print(slots)
-    print(os.listdir("/projects/"+str(slots[slotid]["id"])))
+    #print(os.listdir("/projects/"+str(slots[slotid]["id"])))
     with open("/projects/"+str(slots[slotid]["id"])+"/__init__"+suffix,"rb") as f:
         print("trying to read import program")
         program = f.read()
-        print(program)
+        #print(program)
     try:
         os.remove("/"+module_name+suffix)
     except:
@@ -52,14 +52,6 @@ def importFile(slotid=0, precompiled=False, module_name='importFile'):
 # Dateiname und Modulname sollten gleich sein, dann kann man Code Completion nutzen
 importFile(slotid=6, precompiled=True, module_name="iqrobot")
 import iqrobot as iq
-print(iq.HELLO) 
-
-# Importiere Go Robot Code
-#importFile(slotid=3, precompiled=True, module_name="gorobot")
-#import gorobot as gr
-#gr.exampleFour()
-#gr.db.gyroRotation(90, 25, 35, 25)
-
 
 ################### Hauptcode ####################################
 '''
@@ -74,14 +66,54 @@ dh auch an die Funktionen im importierten Code übergeben werde
 hub = PrimeHub()
 
 # Initialisiere Robot Klasse mit unseren Funktionen
-iqRobot: iq.IQRobot = iq.IQRobot(hub, typ=iq.BRICKIES_BOT)
+iqRobot: iq.IQRobot = iq.IQRobot(hub)
 
 # Führe Funktionen aus unser Robot Klasse aus:
 iqRobot.show('HAPPY')
-iqRobot.fahre_mit_drehung(strecke1=10, grad=90, strecke2=10)
-iqRobot.fahre_mit_drehung(strecke1=0, grad=-90, strecke2=10)
 
+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)
+    iqRobot.fahre_gerade_aus(cm=14,speed=70)
+    iqRobot.fahre_gerade_aus(cm=-13,speed=50)
+    iqRobot.drehe(-45, False)
+    iqRobot.fahre_gerade_aus(cm=-75,speed=50)
 
+def druckmaschine():
+    iqRobot.fahre_gerade_aus(19,30)
+    iqRobot.drehe(-45)  
+    iqRobot.fahre_gerade_aus(20,30)
+    iqRobot.fahre_gerade_aus(-15,30)
 
+def hologram():
+    iqRobot.drehe(45)
+    iqRobot.fahre_gerade_aus(37.5,30)
+    iqRobot.drehe(45)
+    iqRobot.fahre_gerade_aus(15,30)
+    iqRobot.fahre_gerade_aus(-15,30)
 
+def augmented_reality():
+    iqRobot.drehe(-135)
+    iqRobot.fahre_gerade_aus(42,30)
+    iqRobot.drehe(90)
+    iqRobot.fahre_gerade_aus(12,30)
+    iqRobot.schaufel(-100)
+    iqRobot.fahre_gerade_aus(-3,30)
+    iqRobot.drehe(90)
+    iqRobot.fahre_gerade_aus(20,30)
+    iqRobot.drehe(-90)
+    iqRobot.fahre_gerade_aus(5,20)
+
+#iqRobot.fahre_gerade_aus(16, 20)
+#iqRobot.drehe(38)
+#iqRobot.fahre_gerade_aus(33,25)
+iqRobot.schaufel(1600, speed=100 )
+iqRobot.schaufel(-1600, speed=100 )