diff --git a/brickiesbot.py b/brickiesbot.py new file mode 100644 index 0000000..5d27a4e --- /dev/null +++ b/brickiesbot.py @@ -0,0 +1,218 @@ +# 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) + + linker_speed=speed # Geschwindigkeit linker Motor + rechter_speed=speed # Geschwindigkeit rechter Motor + kp = 1.1 # 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 + """ + + 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.") + + + + + + + + + diff --git a/iqrobot.py b/iqrobot.py index 7a37375..24c7d85 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -52,16 +52,16 @@ class IQRobot: 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 @@ -80,12 +80,56 @@ class IQRobot: return # soll der Gierwinkel zurückgesetzt werden? - if with_reset: - self.bewegungsSensor.reset_yaw_angle() # Gierwinkel zurücksetzen + #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 + 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 @@ -94,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 @@ -131,7 +176,8 @@ class IQRobot: 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 @@ -195,7 +241,7 @@ 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=100): """ @@ -216,14 +262,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.") @@ -234,4 +299,3 @@ print("Fertig geladen.") - diff --git a/main copy 2.py b/main copy 2.py new file mode 100644 index 0000000..0f27e6f --- /dev/null +++ b/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) + diff --git a/main copy.py b/main copy.py new file mode 100644 index 0000000..c31ceb2 --- /dev/null +++ b/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() diff --git a/main.py b/main.py index 0f27e6f..836ba18 100644 --- a/main.py +++ b/main.py @@ -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,73 +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(): - 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 hologram_alt(): +# iqRobot.fahre_gerade_aus(cm=75,speed=80) +# iqRobot.drehe(45) +# iqRobot.fahre_gerade_aus(cm=14,speed=70) +# iqRobot.fahre_gerade_aus(cm=-13,speed=50) +# iqRobot.drehe(-45) +# iqRobot.fahre_gerade_aus(cm=-75,speed=50) def druckmaschine(): - iqRobot.fahre_gerade_aus(19,70) + iqRobot.fahre_gerade_aus(19,50) iqRobot.drehe(-45) iqRobot.fahre_gerade_aus(25,50) - iqRobot.fahre_gerade_aus(-15,40) + 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.antrieb.move_tank(15,left_speed=30,right_speed=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(40,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(-2200) - iqRobot.fahre_gerade_aus(-4,30) - iqRobot.drehe(90) - iqRobot.fahre_gerade_aus(20,30) + iqRobot.fahre_gerade_aus(-7,25) # HIER V + iqRobot.drehe(85) + iqRobot.fahre_gerade_aus(20,50) #iqRobot.drehe(-90) #iqRobot.fahre_gerade_aus(5,20) -def backHome(): +# def backHome(): +# iqRobot.fahre_gerade_aus(-5, 30) +# iqRobot.drehe(138) +# iqRobot.fahre_gerade_aus_alt(80, 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(138) - iqRobot.fahre_gerade_aus_alt(80, 100) + 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) -#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 ) +# # Roboter-Design -# druckmaschine() -# hologram() -# augmented_reality() -#backHome() -#huenchenaufgabe() +# 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.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) + +# 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) diff --git a/main_neu.py b/main_neu.py new file mode 100644 index 0000000..8c671b2 --- /dev/null +++ b/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) + + + +