Beschleunigung beim Fahren

Reviewed-on: #20

brickiesbot.py

@@ -0,0 +1,226 @@
+# LEGO type:standard slot:6 autostart
+
+import math
+
+from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor, DistanceSensor
+from spike.control import wait_for_seconds
+
+print("Lade IQ-Bibliothek")
+
+
+'''
+Wir nutzen "Duck typing", dh wir schreiben hinter jede Variabel mit ':' die Klasse, zB `leftMotor: Motor`
+damit man dann später auch wieder Code Completion hat bei Nutzung der Variablen
+'''
+class IQRobot:
+
+    def __init__(self, hub: PrimeHub):
+        
+        self.hub: PrimeHub = hub
+
+        # Radantrieb
+        LEFT_MOTOR_PORT = 'E'
+        RIGHT_MOTOR_PORT = 'F'
+
+        # Motoren für Aufsätze
+        FRONT_MOTOR_RIGHT_PORT = "C"
+        FRONT_MOTOR_LEFT_PORT = "D"
+
+        #self.farbSensor: ColorSensor = ColorSensor("C")
+        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
+        # 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()
+
+        #try: 
+        #
+         #   self.abstandsSensor: DistanceSensor = DistanceSensor("D")
+        #except:
+        #    self.abstandsSensor: DistanceSensor = DistanceSensor("C")
+
+    def show(self, image: str):
+        '''
+        Zeige Bild auf LED Matrix des Spikes
+        image: Bildname wie zB 'HAPPY'
+        '''
+        self.hub.light_matrix.show_image(image)  
+    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 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
+        """
+
+        # 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() # Gierwinkel zurücksetzen
+        
+        #steering = 100 if grad > 0 else -100
+        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.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.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 * richtung # Die Geschwindigkeit soll negativ sein, wenn wir rückwärts fahren
+
+        # Alles zurücksetzen
+        self.linker_motor.set_degrees_counted(0) 
+        self.bewegungsSensor.reset_yaw_angle()
+
+        # Mit irgendeiner Geschwindigkeit g>0 starten. Wert ist irrelevant
+        #self.antrieb.start_tank(10, 10)
+        self.antrieb.set_default_speed(10)
+
+        ziel_speed=speed
+        k_speed=0.04 # Beschleunigung in %
+
+        # Regler Parameter: Nicht ändern!
+        kp = 5.0                # Verstärkungsfaktor zur Regelung
+        ki = 0.5                # Integralfaktor zur Regelung
+        sum_cm = 0              # bereits gefahrene Strecke
+        versatz = 0             # aktueller Versatz
+        sum_versatz = 0         # Summe des Versatzes über Zeit
+
+        self.antrieb.start_tank_at_power(50, 50)
+        speed = min(20, speed)
+        # 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
+        
+            speed = ziel_speed * k_speed + speed * (1 - k_speed)
+
+            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) + ", sum versatz: " + str(sum_versatz))
+
+        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
+        """
+    
+        self.bewegungsSensor.reset_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.antrieb.move(amount=richtung * 0.1, steering=100)
+            drehung = self.bewegungsSensor.get_yaw_angle()
+            print(drehung)
+    
+    # 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))
+            
+    def schaufel(self, rotations, speed):
+        self.linker_motor_vorne.run_for_rotations(rotations, speed)
+        
+print("Fertig geladen.")
+
+
+
+
+
+
+
+
+

iqrobot.py

@@ -26,6 +26,7 @@ class IQRobot:
         FRONT_MOTOR_RIGHT_PORT = "B"
         FRONT_MOTOR_LEFT_PORT = "A"
 
+        self.farbSensor: ColorSensor = ColorSensor("C")
         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)
@@ -45,19 +46,22 @@ class IQRobot:
 
         self.bewegungsSensor: MotionSensor = MotionSensor()
 
-        self.abstandsSensor: DistanceSensor = DistanceSensor("D")
+        try: 
+            self.abstandsSensor: DistanceSensor = DistanceSensor("D")
+        except:
+            self.abstandsSensor: DistanceSensor = DistanceSensor("C")
 
     def show(self, image: str):
-        '''
+        """
         Zeige Bild auf LED Matrix des Spikes
         image: Bildname wie zB 'HAPPY'
-        '''
+        """
         self.hub.light_matrix.show_image(image)
 
     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
 
 
@@ -76,12 +80,56 @@ class IQRobot:
             return
 
         # soll der Gierwinkel zurückgesetzt werden?
-        if with_reset:
+        #if with_reset:
-            self.bewegungsSensor.reset_yaw_angle() # Gierwinkel zurücksetzen
+        #    self.bewegungsSensor.reset_yaw_angle() # Gierwinkel zurücksetzen
         
         #steering = 100 if grad > 0 else -100
         toleranz = 0 # Toleranz soll null sein. Kann erhöht werden, falls der Roboter sich unendlich dreht.
         aktuell = self.bewegungsSensor.get_yaw_angle() # Aktuelle Position
+        if(aktuell + grad > 180):
+            x1=180-aktuell
+            x2=grad-x1
+            x3=-180+x2
+            ziel=x3
+        else:
+            ziel=aktuell+grad
+        #ziel = grad
+        steering = 100 if ziel > aktuell else -100
+        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 :
+            print(str(differenz))
+            aktuell = self.bewegungsSensor.get_yaw_angle()
+            differenz = ziel - aktuell
+            pass
+
+        # stoppe die Bewegung
+        self.antrieb.stop()
+        print ("Final Ziel: {}, Aktuell: {}".format(ziel, aktuell))
+
+    def drehe_bis_winkel(self, grad=90, speed=10):
+        """
+        Funktion um den Roboter auf der Stelle zu drehen
+
+        :param int grad: Grad die der Roboter nachher haben 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 == self.get_grad() :
+            print("Nichts zu tun")
+
+        # soll der Gierwinkel zurückgesetzt werden?
+        #if with_reset:
+        #    self.bewegungsSensor.reset_yaw_angle() # Gierwinkel zurücksetzen
+        
+        #steering = 100 if grad > 0 else -100
+        toleranz = 1 # 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.antrieb.start(steering=steering, speed=speed) # Mit bestimmer Geschwindigkeit starten
@@ -90,6 +138,7 @@ class IQRobot:
 
         # wiederhole solange der Grad der Drehung noch nicht erreicht ist
         while abs(differenz) > toleranz :
+            print(str(differenz))
             aktuell = self.bewegungsSensor.get_yaw_angle()
             differenz = ziel - aktuell
             pass
@@ -98,7 +147,7 @@ class IQRobot:
         self.antrieb.stop()
         print ("Final Ziel: {}, Aktuell: {}".format(ziel, aktuell))
 
-    def fahre_gerade_aus(self, cm, speed=20):
+    def fahre_gerade_aus(self, cm, speed=20, intensity_stop=None, abstand_stop=None, abstand_greater=True):
         """
         Funktion um den Roboter geradeaus fahren zu lassen
 
@@ -122,19 +171,34 @@ class IQRobot:
         self.bewegungsSensor.reset_yaw_angle()
 
         # Mit irgendeiner Geschwindigkeit g>0 starten. Wert ist irrelevant
-        self.antrieb.start_tank(10, 10)
+        self.antrieb.start_tank(richtung*10, richtung*10)
-        self.antrieb.set_default_speed(10)
+        self.antrieb.set_default_speed(richtung*10)
 
         linker_speed=speed      # Geschwindigkeit linker Motor
         rechter_speed=speed     # Geschwindigkeit rechter Motor
-        kp = 1.5                # Verstärkungsfaktor zur Regelung
+        #kp = 1.5                # Verstärkungsfaktor zur Regelung
+        kp = 1.5               # 17.01.2024 ~Vincenz: Runtergesetzt
         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:
+        while sum_cm < cm * richtung:            
+            if intensity_stop:
+                red, green, blue, intensity = self.farbSensor.get_rgb_intensity()
+                print("Farbintensität: " + str(intensity))
+                if intensity == intensity_stop:
+                    break
+            if abstand_stop:
+                abstand = self.abstandsSensor.get_distance_cm()
+                print("Abstand: " + str(abstand))
+                if abstand_greater:
+                    if abstand >= abstand_stop:
+                        break
+                else:
+                    if abstand <= abstand_stop:
+                        break
             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?
@@ -191,9 +255,9 @@ class IQRobot:
         :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
+        self.bothFrontMotors.move_tank(cm*3.3,"cm", speed, speed) # Heber bewegen
 
-    def schaufel(self,prozent, speed=20):
+    def schaufel(self,prozent, speed=100):
         """
         Lässt die Schaufel fahren
 
@@ -212,14 +276,33 @@ class IQRobot:
         :param abstand: Abstand zum Objekt
         :param speed: Geschwindigkeit, mit der gefahren wird
         :param geregelt: Soll mit Regler gefahren werden?
-        """
+        2"""
         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()
+        
+    def clear_light_matrix(self):
+        """
+        """
+        for a in range(5):
+            for b in range(5):
+                self.hub.light_matrix.set_pixel(a, b, 0)
 
+    
+    def wait_for_any_press(self):
+        self.clear_light_matrix()
+        self.hub.light_matrix.set_pixel(4, 4, 100)
+        self.hub.right_button.wait_until_pressed()
+        self.clear_light_matrix()
+
+    def write_light_matrix(self, str):
+        self.hub.light_matrix.write(str)
+
+    def get_grad(self):
+        return self.hub.motion_sensor.get_yaw_angle()
         
 print("Fertig geladen.")
 
@@ -230,4 +313,3 @@ print("Fertig geladen.")
 
 
 
-

main copy 2.py

@@ -0,0 +1,145 @@
+# LEGO type:standard slot:5 autostart
+
+import os, sys
+
+from spike import PrimeHub, LightMatrix, Button, StatusLight, ForceSensor, MotionSensor, Speaker, ColorSensor, App, DistanceSensor, Motor, MotorPair
+from spike.control import wait_for_seconds, wait_until, Timer
+from hub import battery
+from math import *
+
+############## Allgemein: Prüfe Batteriezustand ###############################
+if battery.voltage() < 8000: #set threshold for battery level
+    print("Spannung der Batterie zu niedrig: " + str(battery.voltage()) + " \n"
+        + "--------------------------------------- \n "
+        + "#### UNBEDINGT ROBOTER AUFLADEN !!! #### \n"
+        + "---------------------------------------- \n")
+else:
+    print("Spannung der Batterie " + str(battery.voltage()) + "\n")
+################################################################################
+
+############################## NICHT ÄNDERN ###############################
+def importFile(slotid=0, precompiled=False, module_name='importFile'):
+    print("##### START # IMPORTING CODE FROM SLOT "+str(slotid)+" ##############")
+    import os, sys
+    suffix = ".py"
+    if precompiled:
+        suffix = ".mpy"
+    with open("/projects/.slots","rt") as f:
+        slots = eval(str(f.read()))
+        print(slots)
+    #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)
+    try:
+        os.remove("/"+module_name+suffix)
+    except:
+        pass
+    with open("/"+module_name+suffix,"w+") as f:
+        print("trying to write import program")
+        f.write(program)
+    if (module_name in sys.modules):
+        del sys.modules[module_name]
+    #exec("from " + module_name + " import *")
+    print("##### END # IMPORTING CODE FROM SLOT "+str(slotid)+" ##############")
+#####################################################################################
+
+
+################ Importiere Code aus andere Dateien #################################
+
+# Importiere Code aus der Datei "iqrobot.py"
+# Dateiname und Modulname sollten gleich sein, dann kann man Code Completion nutzen
+importFile(slotid=6, precompiled=True, module_name="iqrobot")
+import iqrobot as iq
+
+################### Hauptcode ####################################
+'''
+Code zum Lösen einer Aufgabe, kann oben importierten Code nutzen
+Es können auch pro Aufgabe eigene Funktionen geschrieben werden
+Wichtig ist, dass die PORTS der Sensoren überall gleich sind
+und auch `hub` als Instanz von PrimeHub
+dh auch an die Funktionen im importierten Code übergeben werde
+'''
+
+# Initialisieren des Hubs, der Aktoren und Sensoren
+hub = PrimeHub()
+
+# Initialisiere Robot Klasse mit unseren Funktionen
+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)
+    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,70)
+    iqRobot.drehe(-45)  
+    iqRobot.fahre_gerade_aus(25,50)
+    iqRobot.fahre_gerade_aus(-15,40)
+
+def hologram():
+    iqRobot.drehe(45)
+    iqRobot.fahre_gerade_aus(37.5,30)
+    iqRobot.fahre_bis_abstand(28,30,True)
+    print(iqRobot.abstandsSensor.get_distance_cm())
+    iqRobot.drehe(45)
+    iqRobot.antrieb.move_tank(15,left_speed=30,right_speed=30)
+    #iqRobot.fahre_gerade_aus(15,30)
+    iqRobot.fahre_gerade_aus(-15,30)
+
+def augmented_reality():
+    iqRobot.drehe(-135)
+    iqRobot.fahre_gerade_aus(40,30)
+    iqRobot.drehe(90)
+    iqRobot.fahre_gerade_aus(12,30)
+    iqRobot.schaufel(-2200)
+    iqRobot.fahre_gerade_aus(-4,30)
+    iqRobot.drehe(90)
+    iqRobot.fahre_gerade_aus(20,30)
+    #iqRobot.drehe(-90)
+    #iqRobot.fahre_gerade_aus(5,20)
+
+def backHome():
+    iqRobot.fahre_gerade_aus(-5, 30)
+    iqRobot.drehe(138)
+    iqRobot.fahre_gerade_aus_alt(80, 100)
+
+
+#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 )
+
+# druckmaschine()
+# hologram()
+# augmented_reality()
+#backHome()
+#huenchenaufgabe()
+
+#iqRobot.farbSensor.light_up(1,1,1)
+
+#wait_for_seconds(2)
+
+while 0 == 0:
+    #print(iqRobot.abstandsSensor.get_distance_cm())
+    #print(iqRobot.farbSensor.get_reflected_light())
+    print(str(iqRobot.farbSensor.get_blue())+" "+str(iqRobot.farbSensor.get_green())+" "+str(iqRobot.farbSensor.get_red()))
+    wait_for_seconds(0.5)
+

main copy.py

@@ -0,0 +1,148 @@
+# LEGO type:standard slot:5 autostart
+
+import os, sys
+
+from spike import PrimeHub, LightMatrix, Button, StatusLight, ForceSensor, MotionSensor, Speaker, ColorSensor, App, DistanceSensor, Motor, MotorPair
+from spike.control import wait_for_seconds, wait_until, Timer
+from hub import battery
+from math import *
+
+############## Allgemein: Prüfe Batteriezustand ###############################
+if battery.voltage() < 7000: #set threshold for battery level
+    print("Spannung der Batterie zu niedrig: " + str(battery.voltage()) + " \n"
+        + "--------------------------------------- \n "
+        + "#### UNBEDINGT ROBOTER AUFLADEN !!! #### \n"
+        + "---------------------------------------- \n")
+else:
+    print("Spannung der Batterie " + str(battery.voltage()) + "\n")
+################################################################################
+
+############################## NICHT ÄNDERN ###############################
+def importFile(slotid=0, precompiled=False, module_name='importFile'):
+    print("##### START # IMPORTING CODE FROM SLOT "+str(slotid)+" ##############")
+    import os, sys
+    suffix = ".py"
+    if precompiled:
+        suffix = ".mpy"
+    with open("/projects/.slots","rt") as f:
+        slots = eval(str(f.read()))
+        print(slots)
+    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)
+    try:
+        os.remove("/"+module_name+suffix)
+    except:
+        pass
+    with open("/"+module_name+suffix,"w+") as f:
+        print("trying to write import program")
+        f.write(program)
+    if (module_name in sys.modules):
+        del sys.modules[module_name]
+    #exec("from " + module_name + " import *")
+    print("##### END # IMPORTING CODE FROM SLOT "+str(slotid)+" ##############")
+#####################################################################################
+
+
+################ Importiere Code aus andere Dateien #################################
+
+# Importiere Code aus der Datei "iqrobot.py"
+# 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 ####################################
+'''
+Code zum Lösen einer Aufgabe, kann oben importierten Code nutzen
+Es können auch pro Aufgabe eigene Funktionen geschrieben werden
+Wichtig ist, dass die PORTS der Sensoren überall gleich sind
+und auch `hub` als Instanz von PrimeHub
+dh auch an die Funktionen im importierten Code übergeben werde
+'''
+
+# Initialisieren des Hubs, der Aktoren und Sensoren
+hub = PrimeHub()
+
+# Initialisiere Robot Klasse mit unseren Funktionen
+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 druckmaschine():
+    iqRobot.fahre_gerade_aus(18,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(38,30)
+    iqRobot.drehe(45)
+    iqRobot.fahre_gerade_aus(12.5,40)
+    iqRobot.fahre_gerade_aus(-15,30)
+
+def augmented_reality():
+    iqRobot.drehe(-135)
+    iqRobot.fahre_gerade_aus(41,30)
+    iqRobot.drehe(90)
+    #iqRobot.fahre_gerade_aus(10,30)
+    #while(True):
+    #    print(str(iqRobot.get_abstand()))
+    iqRobot.fahre_bis_abstand(16)     #hir faren wir mit abstand gegen die wand
+    iqRobot.schaufel(-100)
+    iqRobot.drehe(15)
+    iqRobot.fahre_gerade_aus(-7,30)
+    iqRobot.drehe(30)
+    iqRobot.fahre_gerade_aus(3,20)
+    iqRobot.drehe(20)
+    iqRobot.schaufel(100)
+    iqRobot.fahre_gerade_aus(10,25)
+    iqRobot.drehe(17)
+    iqRobot.fahre_gerade_aus(7.5,30)
+    iqRobot.drehe(-10)
+    iqRobot.fahre_gerade_aus(4,30)
+    iqRobot.drehe(-10)
+    iqRobot.fahre_gerade_aus(3,30)
+    iqRobot.drehe(-10)
+    iqRobot.fahre_gerade_aus(5,20) 
+    iqRobot.drehe(135)
+
+
+def druckmaschine2():
+    iqRobot.fahre_gerade_aus(17,30)
+    iqRobot.drehe(90)
+    iqRobot.fahre_gerade_aus(17.5)
+    iqRobot.drehe(-138)
+    #iqRobot.fahre_gerade_aus(-2)
+    iqRobot.fahre_bis_abstand(9.5)
+    iqRobot.schaufel(-90, speed=5)
+    iqRobot.fahre_gerade_aus(1.4,20)
+    iqRobot.schaufel(40,speed=5)
+    iqRobot.fahre_gerade_aus(0.6,20)
+    iqRobot.schaufel(75,speed=10)
+
+
+#druckmaschine()
+#hologram()
+#augmented_reality()
+druckmaschine2()

main.py

@@ -1,4 +1,4 @@
-# LEGO type:standard slot:5 autostart
+# LEGO type:standard slot:5
 
 import os, sys
 
@@ -26,10 +26,10 @@ def importFile(slotid=0, precompiled=False, module_name='importFile'):
         suffix = ".mpy"
     with open("/projects/.slots","rt") as f:
         slots = eval(str(f.read()))
-        print(slots)
+        #print(slots)
     #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")
+        #print("trying to read import program")
         program = f.read()
         #print(program)
     try:
@@ -37,7 +37,7 @@ def importFile(slotid=0, precompiled=False, module_name='importFile'):
     except:
         pass
     with open("/"+module_name+suffix,"w+") as f:
-        print("trying to write import program")
+        #print("trying to write import program")
         f.write(program)
     if (module_name in sys.modules):
         del sys.modules[module_name]
@@ -73,47 +73,185 @@ iqRobot.show('HAPPY')
 
 def huenchenaufgabe():
     iqRobot.fahre_gerade_aus(40,60)
-    iqRobot.drehe(-40,True)
+    iqRobot.drehe(-40)
     iqRobot.fahre_gerade_aus(20,60)
     iqRobot.drehe(-20)
     iqRobot.fahre_gerade_aus(55,60)
     iqRobot.heber(10,30)
 
-def hologram_alt():
+# def hologram_alt():
-    iqRobot.fahre_gerade_aus(cm=75,speed=80)
+#     iqRobot.fahre_gerade_aus(cm=75,speed=80)
-    iqRobot.drehe(45, False)
+#     iqRobot.drehe(45)
-    iqRobot.fahre_gerade_aus(cm=14,speed=70)
+#     iqRobot.fahre_gerade_aus(cm=14,speed=70)
-    iqRobot.fahre_gerade_aus(cm=-13,speed=50)
+#     iqRobot.fahre_gerade_aus(cm=-13,speed=50)
-    iqRobot.drehe(-45, False)
+#     iqRobot.drehe(-45)
-    iqRobot.fahre_gerade_aus(cm=-75,speed=50)
+#     iqRobot.fahre_gerade_aus(cm=-75,speed=50)
 
 def druckmaschine():
-    iqRobot.fahre_gerade_aus(19,30)
+    iqRobot.fahre_gerade_aus(19,50)
     iqRobot.drehe(-45)  
-    iqRobot.fahre_gerade_aus(20,30)
+    iqRobot.fahre_gerade_aus(25,50)
-    iqRobot.fahre_gerade_aus(-15,30)
+    iqRobot.fahre_gerade_aus(-15,50)
 
 def hologram():
     iqRobot.drehe(45)
-    iqRobot.fahre_gerade_aus(37.5,30)
+    iqRobot.fahre_gerade_aus(37.5,50) # HIER V
+    iqRobot.fahre_bis_abstand(28,30,True)
+    print(iqRobot.abstandsSensor.get_distance_cm())
     iqRobot.drehe(45)
-    iqRobot.fahre_gerade_aus(15,30)
+    iqRobot.antrieb.move_tank(15,left_speed=50,right_speed=50)
-    iqRobot.fahre_gerade_aus(-15,30)
+    #iqRobot.fahre_gerade_aus(15,30)
+    iqRobot.fahre_gerade_aus(-15,50)
 
 def augmented_reality():
     iqRobot.drehe(-135)
-    iqRobot.fahre_gerade_aus(42,30)
+    iqRobot.fahre_gerade_aus(38.5,40) # HIER V
     iqRobot.drehe(90)
-    iqRobot.fahre_gerade_aus(12,30)
+    iqRobot.fahre_gerade_aus(15.5,30)
-    iqRobot.schaufel(-100)
+    iqRobot.schaufel(-2200)
-    iqRobot.fahre_gerade_aus(-3,30)
+    iqRobot.fahre_gerade_aus(-7,25) # HIER V
-    iqRobot.drehe(90)
+    iqRobot.drehe(85)
-    iqRobot.fahre_gerade_aus(20,30)
+    iqRobot.fahre_gerade_aus(20,50)
-    iqRobot.drehe(-90)
+    #iqRobot.drehe(-90)
-    iqRobot.fahre_gerade_aus(5,20)
+    #iqRobot.fahre_gerade_aus(5,20)
 
-#iqRobot.fahre_gerade_aus(16, 20)
+# def backHome():
-#iqRobot.drehe(38)
+#     iqRobot.fahre_gerade_aus(-5, 30)
-#iqRobot.fahre_gerade_aus(33,25)
+#     iqRobot.drehe(138)
-iqRobot.schaufel(1600, speed=100 )
+#     iqRobot.fahre_gerade_aus_alt(80, 100)
-iqRobot.schaufel(-1600, speed=100 )
+
+def immersiv_experiene():
+    iqRobot.fahre_gerade_aus(-62, 50)
+    iqRobot.schaufel(2200)
+    iqRobot.drehe(-30)
+    iqRobot.fahre_gerade_aus_alt(-2, 10)
+    iqRobot.drehe(-60)
+    iqRobot.fahre_bis_abstand(8, 20, True)
+    iqRobot.schaufel(-4000, 50)
+    #iqRobot.bewegungsSensor.reset_yaw_angle()
+    #iqRobot.drehe(90, True)
+    #iqRobot.drehe(90)
+    #iqRobot.fahre_gerade_aus(40, 70)
+    #iqRobot.fahre_bis_abstand(94, 50, True)
+
+def druckmaschine_2():
+    iqRobot.fahre_gerade_aus(-11, 30)
+    iqRobot.drehe(49)
+    iqRobot.schaufel(500)
+    iqRobot.fahre_gerade_aus_alt(6, 30)
+    iqRobot.schaufel(300)
+    iqRobot.fahre_gerade_aus_alt(5, 30)
+    iqRobot.schaufel(200)
+
+# def huenchenaufgabe_neu():
+#     iqRobot.fahre_gerade_aus(70, 50)
+#     iqRobot.drehe(-90)
+#     iqRobot.fahre_gerade_aus(35, 30)  
+#     iqRobot.drehe(35)   
+#     iqRobot.fahre_gerade_aus(30, 30)
+#     #iqRobot.fahre_gerade_aus(-30, 30)
+#     #iqRobot.drehe(-35)
+#     #iqRobot.fahre_gerade_aus(-35, 30)
+#     #iqRobot.drehe(90)
+#     #iqRobot.fahre_gerade_aus(-70, 50)
+
+def huenchenaufgabe_neu_besser():
+    iqRobot.fahre_gerade_aus(51, 50)
+    iqRobot.drehe(-55)
+    iqRobot.fahre_gerade_aus(78, 70)
+    # iqRobot.heber(100, 500)
+
+def turm(startGrad):
+    #iqRobot.fahre_gerade_aus_alt(-3, 100)
+    iqRobot.antrieb.move_tank(-9, "cm", 30, 30)
+    iqRobot.heber(50, 500) # NEU!
+    iqRobot.drehe(15)
+    #iqRobot.fahre_gerade_aus_alt(3, 100)
+    iqRobot.antrieb.move_tank(5, "cm", 30, 30)
+    iqRobot.drehe(90)
+    # iqRobot.heber(-100, 500)
+    iqRobot.antrieb.move_tank(3, "cm", 30, 30)
+    #iqRobot.drehe(36)
+    print(startGrad)
+    #wait_for_seconds(5)
+    #iqRobot.drehe_bis_winkel(startGrad+88)
+    #wait_for_seconds(2)
+    ##iqRobot.drehe_bis_winkel(startGrad+88)
+    #wait_for_seconds(2)
+    #iqRobot.fahre_gerade_aus_alt(15, 50)
+    iqRobot.drehe(41)
+    iqRobot.antrieb.move_tank(15.25, "cm", 50, 50)
+    # wait_for_seconds(2)
+    #iqRobot.drehe_bis_winkel(startGrad+90)
+    #iqRobot.fahre_bis_abstand(5, 50, False)
+    iqRobot.heber(275, 500)
+
+def boot():
+    iqRobot.fahre_gerade_aus(50, 70)
+    iqRobot.drehe(90)
+    iqRobot.fahre_gerade_aus(10, 40)
+    iqRobot.drehe(-90)
+    iqRobot.fahre_gerade_aus(110, 90)
+
+def toHome1():
+    iqRobot.fahre_gerade_aus(-5, 30)
+    iqRobot.drehe(-20)
+    iqRobot.fahre_gerade_aus(30, 70)
+    iqRobot.drehe(40)
+    #iqRobot.fahre_gerade_aus_alt(25, 50)
+    iqRobot.antrieb.start_tank(40, 40)
+    iqRobot.wait_for_any_press()
+    iqRobot.antrieb.stop()
+
+####################################################
+
+# Teil 1
+iqRobot.wait_for_any_press()
+iqRobot.write_light_matrix("1")
+druckmaschine()
+iqRobot.write_light_matrix("2")
+hologram()
+iqRobot.write_light_matrix("3")
+augmented_reality()
+iqRobot.write_light_matrix("4")
+druckmaschine_2()
+toHome1()
+wait_for_seconds(3)
+
+# # Teil 2
+iqRobot.wait_for_any_press()
+boot()
+
+# Teil 3
+iqRobot.wait_for_any_press()
+grad_am_start = iqRobot.get_grad()
+print(grad_am_start)
+iqRobot.write_light_matrix("5")
+huenchenaufgabe_neu_besser()
+iqRobot.write_light_matrix("6")
+turm(grad_am_start)
+
+
+# # Roboter-Design
+
+# wait_for_seconds(2)
+# iqRobot.wait_for_any_press()
+# iqRobot.fahre_gerade_aus(40, 35)
+# iqRobot.wait_for_any_press()
+# iqRobot.fahre_gerade_aus(40, 35)
+
+
+
+
+
+# iqRobot.wait_for_any_press()
+# grad_am_start = iqRobot.get_grad()
+# print("WARTEN 2")
+# wait_for_seconds(3)
+# iqRobot.drehe_bis_winkel(grad_am_start+90)
+
+# # while True:
+#     print(iqRobot.abstandsSensor.get_distance_cm())
+#     #print(iqRobot.farbSensor.get_reflected_light())
+#     #print(str(iqRobot.farbSensor.get_blue())+" "+str(iqRobot.farbSensor.get_green())+" "+str(iqRobot.farbSensor.get_red()))
+#     wait_for_seconds(0.5)

main_neu.py

@@ -0,0 +1,100 @@
+# LEGO type:standard slot:5 autostart
+
+import os, sys
+
+from spike import PrimeHub, LightMatrix, Button, StatusLight, ForceSensor, MotionSensor, Speaker, ColorSensor, App, DistanceSensor, Motor, MotorPair
+from spike.control import wait_for_seconds, wait_until, Timer
+from hub import battery
+from math import *
+
+
+############################## NICHT ÄNDERN ###############################
+def importFile(slotid=0, precompiled=False, module_name='importFile'):
+    print("##### START # IMPORTING CODE FROM SLOT "+str(slotid)+" ##############")
+    import os, sys
+    suffix = ".py"
+    if precompiled:
+        suffix = ".mpy"
+    with open("/projects/.slots","rt") as f:
+        slots = eval(str(f.read()))
+        #print(slots)   
+    #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)
+    try:
+        os.remove("/"+module_name+suffix)
+    except:
+        pass
+    with open("/"+module_name+suffix,"w+") as f:
+        #print("trying to write import program")
+        f.write(program)
+    if (module_name in sys.modules):
+        del sys.modules[module_name]
+    #exec("from " + module_name + " import *")
+    print("##### END # IMPORTING CODE FROM SLOT "+str(slotid)+" ##############")
+#####################################################################################
+
+
+################ Importiere Code aus andere Dateien #################################
+
+# Importiere Code aus der Datei "iqrobot.py"
+# Dateiname und Modulname sollten gleich sein, dann kann man Code Completion nutzen
+importFile(slotid=6, precompiled=True, module_name="brickiesbot")
+import brickiesbot as bot
+
+################### Hauptcode ####################################
+'''
+Code zum Lösen einer Aufgabe, kann oben importierten Code nutzen
+Es können auch pro Aufgabe eigene Funktionen geschrieben werden
+Wichtig ist, dass die PORTS der Sensoren überall gleich sind
+und auch `hub` als Instanz von PrimeHub
+dh auch an die Funktionen im importierten Code übergeben werde
+'''
+
+# Initialisieren des Hubs, der Aktoren und Sensoren
+hub = PrimeHub()
+
+# Initialisiere Robot Klasse mit unseren Funktionen
+brickiesBot: bot.IQRobot = bot.IQRobot(hub)
+
+#Druckmaschiene1
+brickiesBot.show('HAPPY')
+brickiesBot.fahre_gerade_aus(21,50)
+brickiesBot.drehe(-45)
+brickiesBot.fahre_gerade_aus(22,50)
+#Hologramm
+brickiesBot.fahre_gerade_aus(-20,50)
+brickiesBot.drehe(45)
+brickiesBot.fahre_gerade_aus(43)
+brickiesBot.drehe(45)
+brickiesBot.fahre_gerade_aus(12,50)
+brickiesBot.fahre_gerade_aus(-12,50)
+#AGRealiy
+brickiesBot.drehe(-135)
+brickiesBot.fahre_gerade_aus(43,50)
+brickiesBot.drehe(90)
+brickiesBot.fahre_gerade_aus(12,50)
+brickiesBot.schaufel(0.53, 30)
+brickiesBot.fahre_gerade_aus(-8,50)
+brickiesBot.drehe(90)
+brickiesBot.fahre_gerade_aus(14,50)
+brickiesBot.drehe(-60)
+brickiesBot.fahre_gerade_aus(1.5,50)
+brickiesBot.fahre_gerade_aus(-1.5,50)
+brickiesBot.drehe(60)
+brickiesBot.fahre_gerade_aus(-16,50)
+brickiesBot.drehe(45)
+#Drukmaschiene2
+brickiesBot.fahre_gerade_aus(15)
+brickiesBot.schaufel(0.33, 30)
+brickiesBot.fahre_gerade_aus(6)
+brickiesBot.schaufel(0.45, 200)
+#lichtshow
+brickiesBot.fahre_gerade_aus(-20, 50)
+brickiesBot.drehe(-45)
+
+
+
+