From 127137e79f9abbc76c6adb4a1cee6cb9752ff92d Mon Sep 17 00:00:00 2001 From: Lars Haferkamp <> Date: Wed, 27 Sep 2023 21:00:25 +0200 Subject: [PATCH 01/31] WIP: Fahren mit Regler --- iqrobot.py | 70 ++++++++++++++-- iqrobot_schreiber.py | 187 +++++++++++++++++++++++++++++++++++++++++++ main.py | 5 +- 3 files changed, 253 insertions(+), 9 deletions(-) create mode 100644 iqrobot_schreiber.py diff --git a/iqrobot.py b/iqrobot.py index 1595e5b..a26d986 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -1,6 +1,8 @@ # LEGO type:standard slot:6 autostart -from spike import PrimeHub, Motor, MotorPair, ColorSensor +import math + +from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor from spike.control import wait_for_seconds HELLO = "HELLO IQ" @@ -13,10 +15,35 @@ class IQRobot: def __init__(self, hub: PrimeHub, leftMotorPort: str, rightMotorPort: str, colorSensorPort: str): self.hub: PrimeHub = hub - self.leftMotor: Motor = Motor(leftMotorPort) - self.rightMotor: Motor = Motor(rightMotorPort) - self.movementMotors: MotorPair = MotorPair(leftMotorPort, rightMotorPort) - self.colorSensor: ColorSensor = ColorSensor(colorSensorPort) + self.typ=typ + if self.typ=="backstein": + LEFT_MOTOR_PORT = 'F' + RIGHT_MOTOR_PORT = 'B' + FRONT_MOTOR_RIGHT_PORT = "E" + self.frontMotorRight: Motor = Motor(FRONT_MOTOR_RIGHT_PORT) + self.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT) + elif self.typ=="brickies": + LEFT_MOTOR_PORT = 'E' + RIGHT_MOTOR_PORT = 'F' + 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) + self.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT) + # Radumfang neu berechnen und Motor konfigurieren + rad_radius = 2.1 + rad_umfang = 2 * math.pi * rad_radius + self.movementMotors.set_motor_rotation(rad_umfang) + # Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters + self.abstand_rad_front = 5.55 + + 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() def show(self, image: str): @@ -35,11 +62,42 @@ class IQRobot: def getColorIntensity(self): # Ermittele Farbintensität über den Farbsensor - (red, green, blue, colorIntensity) = self.colorSensor.get_rgb_intensity() + (red, green, blue, colorIntensity) = self .colorSensor.get_rgb_intensity() return colorIntensity + 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) + + 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) + + 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 + + self.movementMotors.move(cm - sum_cm) + + def fahre_mit_drehung(self, strecke1, grad, strecke2): + self.fahre_gerade_geregelt(strecke1 + self.abstand_rad_front) + self.drehe(grad) + self.fahre_gerade_geregelt(strecke2 - self.abstand_rad_front) print("successfully loaded the IQ Lego teams code :)") diff --git a/iqrobot_schreiber.py b/iqrobot_schreiber.py new file mode 100644 index 0000000..404e187 --- /dev/null +++ b/iqrobot_schreiber.py @@ -0,0 +1,187 @@ +# LEGO type:standard slot:7 autostart + +######################################################################## +# "ALTE" VERSION MIT DER WIR VERSUCHT HABEN DAS WORT "LEGO" ZU SCHREIBEN +######################################################################## + +from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor +from spike.control import wait_for_seconds + +HELLO = "HELLO IQ 2" + +''' +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, colorSensorPort: str, typ: str): + self.hub: PrimeHub = hub + self.typ=typ + if self.typ=="backstein": + LEFT_MOTOR_PORT = 'F' + RIGHT_MOTOR_PORT = 'B' + FRONT_MOTOR_RIGHT_PORT = "E" + self.frontMotorRight: Motor = Motor(FRONT_MOTOR_RIGHT_PORT) + elif self.typ=="brickies": + LEFT_MOTOR_PORT = 'E' + RIGHT_MOTOR_PORT = 'F' + 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) + + self.leftMotor: Motor = Motor(LEFT_MOTOR_PORT) + self.rightMotor: Motor = Motor(RIGHT_MOTOR_PORT) + self.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT) + #self.colorSensor: ColorSensor = ColorSensor(colorSensorPort) + #self.frontMotorLeft: Motor = Motor("C") + self.motionSensor: MotionSensor = MotionSensor() + + + 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 driveForward_for_sec(self, seconds: float): + # Fahre die übergebene Anzahl seconds gerade aus + self.movementMotors.start() + wait_for_seconds(seconds) + self.movementMotors.stop() + + 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): + if with_reset: + self.motionSensor.reset_yaw_angle() + steering = 100 if grad > 0 else -100 + self.movementMotors.start(steering=steering, speed=10) + while abs(self.motionSensor.get_yaw_angle()) < abs(grad): + pass + self.movementMotors.stop() + + + def drehe_robot(self, grad=90): + if self.typ == "backstein": + radius=9.5 + stift_versatz=2.2 + if self.typ == "brickies": + radius=17.4 + stift_versatz=0.3 + self.fahre_gerade(-radius - stift_versatz) + self.drehe(grad) + self.fahre_gerade(radius - stift_versatz) + + def fahre_gerade(self, cm, zeichne=False): + if zeichne: + self.bewege_stift(1) # Stift runter + self.motionSensor.reset_yaw_angle() + if self.typ == "brickies": + cm = -cm + self.movementMotors.move(cm) + if zeichne: + self.bewege_stift(-1) # Stift hoch + versatz = self.motionSensor.get_yaw_angle() + self.drehe(grad=-versatz) + + def buchstabe_zu_segmenten(self, buchstabe): + # Segmente um Buchstaben zu schreiben + # 4_ + # 5 |__|3 + # 0 |6_|2 + # 1 + # + buchstabe_zu_segmenten = {"L": [1,1,0,0,0,1,0], "E": [1,1,0,0,1,1,1], "G": [1,1,1,0,1,1,0], "O": [1,1,1,1,1,1,0]} + return buchstabe_zu_segmenten[buchstabe] + + + def bewege_stift(self, richtung): + if self.typ == "backstein": + self.frontMotorRight.run_for_rotations(richtung*0.4) + if self.typ == "brickies": + #print("bewege stift brickies") + self.bothFrontMotors.move(-richtung*0.2, unit='rotations', speed=5) + + def schreibe_buchstabe(self, buchstabe): + print("Schreibe " + buchstabe) + segmente = self.buchstabe_zu_segmenten(buchstabe) + grad_drehung=-90 + self.fahre_gerade(2) + self.drehe_robot(-grad_drehung) # drehe rechts + for segment_nummer, segment in enumerate(segmente): + print("Segment: " + str(segment) + " , Segment Nummer: " + str(segment_nummer)) + if segment==1: + self.fahre_gerade(5, zeichne=True) + else: + self.fahre_gerade(5) + if segment_nummer != 2 and segment_nummer != 6: + self.drehe_robot(grad_drehung) # drehe links + + + def schreibeL(self, schreibe=True, zurueck=False): + if zurueck: + step = 5 + faktor = -1 + else: + step = 1 + faktor = 1 + print("Schreibe L") + #self.frontMotorRight.run_for_rotations(-0.4) + radius=9.5 + stift_versatz=2.2 + if schreibe: + self.frontMotorRight.run_for_rotations(0.4) + self.movementMotors.set_default_speed(10) + + while (True): + if step == 0: + break + if step == 1: + self.movementMotors.move(faktor * 5) + if schreibe: + self.frontMotorRight.run_for_rotations(-0.4) + if step == 2: + self.movementMotors.move(faktor * (-radius - stift_versatz)) + if step == 3: + self.drehe(faktor * -90) + if step == 4: + self.movementMotors.move(faktor*(radius - stift_versatz)) + if schreibe: + self.frontMotorRight.run_for_rotations(0.4) + if step == 5: + self.movementMotors.move(faktor * 2) + if schreibe: + self.frontMotorRight.run_for_rotations(-0.4) + if step == 6: + break + step += faktor + # Fahre 5 cm rückwerts + # dann drehe nach rechts 90° + # und fahre 2cm fohrwärts + #stift hoch + + + def schreibeLego(self): + #self.schreibeL() + #self.schreibeL(schreibe=False, zurueck=True) + self.movementMotors.set_default_speed(10) + self.bewege_stift(-1) + self.fahre_gerade(4, zeichne=True) + self.drehe_robot() + self.fahre_gerade(4, zeichne=True) + #self.schreibe_buchstabe("L") + #self.schreibe_buchstabe("E") + #self.schreibe_buchstabe("G") + #self.schreibe_buchstabe("O") + +print("successfully loaded the IQ Lego teams code :)") + + diff --git a/main.py b/main.py index c6ba5c2..d271f57 100644 --- a/main.py +++ b/main.py @@ -83,9 +83,8 @@ iqRobot: iq.IQRobot = iq.IQRobot(hub, LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT, COLOR_S # Führe Funktionen aus unser Robot Klasse aus: iqRobot.show('HAPPY') -iqRobot.driveForward_for_sec(2.0) -colorIntensity = iqRobot.getColorIntensity() -print("Farbintensität: " + str(colorIntensity)) +iqRobot.fahre_mit_drehung(strecke1=10, grad=90, strecke2=10) +iqRobot.fahre_mit_drehung(strecke1=0, grad=-90, strecke2=10) From 833224793a7903b8fb0c9bca399e11653d079092 Mon Sep 17 00:00:00 2001 From: Lars Haferkamp <> Date: Thu, 5 Oct 2023 08:56:46 +0200 Subject: [PATCH 02/31] Fixed and refactored --- iqrobot.py | 54 ++++++++++++++++++++++++++++++++++++++++++------------ main.py | 7 +------ 2 files changed, 43 insertions(+), 18 deletions(-) diff --git a/iqrobot.py b/iqrobot.py index a26d986..e815c5f 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -6,6 +6,9 @@ from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor from spike.control import wait_for_seconds HELLO = "HELLO IQ" +BRICKIES_BOT = "brickies" +BRICKIES_BOT_2 = "brickies_2" +BACKSTEIN_BOT = "backstein" ''' Wir nutzen "Duck typing", dh wir schreiben hinter jede Variabel mit ':' die Klasse, zB `leftMotor: Motor` @@ -13,34 +16,50 @@ damit man dann später auch wieder Code Completion hat bei Nutzung der Variablen ''' class IQRobot: - def __init__(self, hub: PrimeHub, leftMotorPort: str, rightMotorPort: str, colorSensorPort: str): + def __init__(self, hub: PrimeHub, typ: str): + self.hub: PrimeHub = hub self.typ=typ - if self.typ=="backstein": + + 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) - self.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT) - elif self.typ=="brickies": + + 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) - self.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT) - # Radumfang neu berechnen und Motor konfigurieren - rad_radius = 2.1 - rad_umfang = 2 * math.pi * rad_radius - self.movementMotors.set_motor_rotation(rad_umfang) + # 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) + # 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.rightMotor: Motor = Motor(RIGHT_MOTOR_PORT) #self.colorSensor: ColorSensor = ColorSensor(colorSensorPort) #self.frontMotorLeft: Motor = Motor("C") self.motionSensor: MotionSensor = MotionSensor() @@ -60,12 +79,23 @@ class IQRobot: wait_for_seconds(seconds) self.movementMotors.stop() + 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): + if with_reset: + self.motionSensor.reset_yaw_angle() + steering = 100 if grad > 0 else -100 + self.movementMotors.start(steering=steering, speed=10) + while abs(self.motionSensor.get_yaw_angle()) < abs(grad): + pass + self.movementMotors.stop() + + def fahre_gerade(self, cm): if self.typ == "brickies": cm = -cm diff --git a/main.py b/main.py index d271f57..296fcf8 100644 --- a/main.py +++ b/main.py @@ -70,16 +70,11 @@ und auch `hub` als Instanz von PrimeHub dh auch an die Funktionen im importierten Code übergeben werde ''' -# Definiere an welchen Ports die Sensoren angeschlossen sind -COLOR_SENSOR_PORT = 'E' -LEFT_MOTOR_PORT = 'A' -RIGHT_MOTOR_PORT = 'B' - # Initialisieren des Hubs, der Aktoren und Sensoren hub = PrimeHub() # Initialisiere Robot Klasse mit unseren Funktionen -iqRobot: iq.IQRobot = iq.IQRobot(hub, LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT, COLOR_SENSOR_PORT) +iqRobot: iq.IQRobot = iq.IQRobot(hub, typ=iq.BRICKIES_BOT) # Führe Funktionen aus unser Robot Klasse aus: iqRobot.show('HAPPY') From ad4b131af6041d0c2487847c696c74112d89f884 Mon Sep 17 00:00:00 2001 From: unknown Date: Wed, 25 Oct 2023 19:27:02 +0200 Subject: [PATCH 03/31] =?UTF-8?q?Funktionen=20drehe=20angepasst,=20fahre?= =?UTF-8?q?=5Fgerade=5Faus=20hinzugef=C3=BCgt?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- iqrobot.py | 40 +++++++++++++++++++++++++++++++++++++--- 1 file changed, 37 insertions(+), 3 deletions(-) diff --git a/iqrobot.py b/iqrobot.py index e815c5f..3aa2ed9 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -87,13 +87,25 @@ class IQRobot: def drehe(self, grad=90, with_reset=True): + if grad == 0 or grad == 360 : + print("nichts zu tun") + return if with_reset: self.motionSensor.reset_yaw_angle() - steering = 100 if grad > 0 else -100 + #steering = 100 if grad > 0 else -100 + toleranz = 0 + aktuell = self.motionSensor.get_yaw_angle() + ziel = grad + steering = 100 if ziel > aktuell else -100 self.movementMotors.start(steering=steering, speed=10) - while abs(self.motionSensor.get_yaw_angle()) < abs(grad): + differenz = ziel - aktuell + print ("Start Ziel: {}, Aktuell: {}".format(ziel, aktuell)) + while abs(differenz) > toleranz : + aktuell = self.motionSensor.get_yaw_angle() + differenz = ziel - aktuell pass self.movementMotors.stop() + print ("Final Ziel: {}, Aktuell: {}".format(ziel, aktuell)) def fahre_gerade(self, cm): @@ -128,8 +140,30 @@ 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(self, cm,speed): + + self.motionSensor.reset_yaw_angle() + + self.movementMotors.move_tank(amount=cm,left_speed=speed, right_speed=speed) + drehung = self.motionSensor.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() + print(drehung) - + print("successfully loaded the IQ Lego teams code :)") + + + + + + From eff2b56555612098ca4c1879cfbe8643e7598de0 Mon Sep 17 00:00:00 2001 From: "(MakerLab) Laptop 2" Date: Wed, 8 Nov 2023 18:51:37 +0100 Subject: [PATCH 04/31] Aufgabe 07: Hologramm --- main.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/main.py b/main.py index 296fcf8..682b717 100644 --- a/main.py +++ b/main.py @@ -74,14 +74,17 @@ 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, typ=iq.BRICKIES_BOT_2) # 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 hologram_aufgabe1(): + 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) +hologram_aufgabe1() From d7026c60ad1ae087b76ffc466c63e33f77e7ca77 Mon Sep 17 00:00:00 2001 From: unknown Date: Wed, 8 Nov 2023 19:03:45 +0100 Subject: [PATCH 05/31] TEst --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index baba04a..d7c0858 100644 --- a/README.md +++ b/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; From 68f71f808f104a8542dbb5e58820ca4ad7bb2415 Mon Sep 17 00:00:00 2001 From: "(MakerLab) Laptop 2" Date: Wed, 15 Nov 2023 18:42:04 +0100 Subject: [PATCH 06/31] Schaufel Funktion --- iqrobot.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/iqrobot.py b/iqrobot.py index 3aa2ed9..c542e4c 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -157,6 +157,11 @@ class IQRobot: drehung = self.motionSensor.get_yaw_angle() print(drehung) + def schaufel(self,prozent): + volle_umdrehung=0.29 + rotations=volle_umdrehung*prozent/100 + self.bothFrontMotors.move(rotations, unit='rotations',speed=20) + print("successfully loaded the IQ Lego teams code :)") From 58477b1d548f3249f6b151fbb3a92407d79dd6ba Mon Sep 17 00:00:00 2001 From: Laptop 3 Date: Wed, 15 Nov 2023 18:42:59 +0100 Subject: [PATCH 07/31] Heber Funktion --- iqrobot.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/iqrobot.py b/iqrobot.py index 3aa2ed9..4be7d7b 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -52,7 +52,7 @@ class IQRobot: 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) # Radumfang neu berechnen und Motor konfigurieren @@ -156,7 +156,8 @@ class IQRobot: self.movementMotors.move(amount=richtung * 0.1, steering=100) drehung = self.motionSensor.get_yaw_angle() print(drehung) - + def heber(self, cm,speed): + self.bothFrontMotors.move_tank(-cm*3.3,"cm", -speed, speed) print("successfully loaded the IQ Lego teams code :)") From 33369ff3e2f5560e095571f49f44e0e75d8aeaf1 Mon Sep 17 00:00:00 2001 From: Lars Haferkamp <> Date: Thu, 16 Nov 2023 09:26:31 +0100 Subject: [PATCH 08/31] Ungenutzten Code/Robot Config entfernt und etwas dokumentiert --- iqrobot.py | 148 +++++++++++++++++++++-------------------------------- main.py | 2 +- 2 files changed, 60 insertions(+), 90 deletions(-) diff --git a/iqrobot.py b/iqrobot.py index 3aa2ed9..6a4791c 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -6,9 +6,6 @@ from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor from spike.control import wait_for_seconds HELLO = "HELLO IQ" -BRICKIES_BOT = "brickies" -BRICKIES_BOT_2 = "brickies_2" -BACKSTEIN_BOT = "backstein" ''' Wir nutzen "Duck typing", dh wir schreiben hinter jede Variabel mit ':' die Klasse, zB `leftMotor: Motor` @@ -16,53 +13,30 @@ 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.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.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() + self.antrieb.set_motor_rotation(rad_umfang) + + self.bewegungsSensor: MotionSensor = MotionSensor() def show(self, image: str): @@ -73,88 +47,84 @@ 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 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): + """ + 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 + """ if grad == 0 or grad == 360 : print("nichts zu tun") return if with_reset: - self.motionSensor.reset_yaw_angle() + self.bewegungsSensor.reset_yaw_angle() #steering = 100 if grad > 0 else -100 toleranz = 0 - aktuell = self.motionSensor.get_yaw_angle() + aktuell = self.bewegungsSensor.get_yaw_angle() ziel = grad steering = 100 if ziel > aktuell else -100 - self.movementMotors.start(steering=steering, speed=10) + self.antrieb.start(steering=steering, speed=10) differenz = ziel - aktuell print ("Start Ziel: {}, Aktuell: {}".format(ziel, aktuell)) while abs(differenz) > toleranz : - aktuell = self.motionSensor.get_yaw_angle() + aktuell = self.bewegungsSensor.get_yaw_angle() differenz = ziel - aktuell pass - self.movementMotors.stop() + self.antrieb.stop() print ("Final Ziel: {}, Aktuell: {}".format(ziel, aktuell)) - - - 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) - + 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) + """ + WIP by Lars & Klaus + """ + self.bewegungsSensor.reset_yaw_angle() + self.antrieb.start_tank(10, 10) + self.antrieb.set_default_speed(10) sum_cm = 0 while sum_cm < cm: - self.movementMotors.move(1) - versatz = self.motionSensor.get_yaw_angle() + self.antrieb.move(1) + versatz = self.bewegungsSensor.get_yaw_angle() self.drehe(grad=-versatz) - self.motionSensor.reset_yaw_angle() + self.bewegungsSensor.reset_yaw_angle() sum_cm = sum_cm + 1 - self.movementMotors.move(cm - sum_cm) + self.antrieb.move(cm - sum_cm) + 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) - def fahre_gerade_aus(self, cm,speed): + + def fahre_gerade_aus(self, cm: float, speed: int): + """ + Funktion zum gerade aus fahren mit Korrektur am Ende + + :param int cm: Zentimeter die gerade aus gefahren werden soll + :param speed: Geschwindigkeit mit der gefahren wird + """ - self.motionSensor.reset_yaw_angle() + self.bewegungsSensor.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) diff --git a/main.py b/main.py index 682b717..cffdfc7 100644 --- a/main.py +++ b/main.py @@ -74,7 +74,7 @@ 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_2) +iqRobot: iq.IQRobot = iq.IQRobot(hub) # Führe Funktionen aus unser Robot Klasse aus: iqRobot.show('HAPPY') From 83970cc0532d97808b674981397cb226b9087c21 Mon Sep 17 00:00:00 2001 From: "(MakerLab) Laptop 2" Date: Wed, 22 Nov 2023 17:14:36 +0100 Subject: [PATCH 09/31] erste Aufgaben --- main.py | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/main.py b/main.py index cffdfc7..c1fd649 100644 --- a/main.py +++ b/main.py @@ -87,4 +87,34 @@ def hologram_aufgabe1(): iqRobot.drehe(-45, False) iqRobot.fahre_gerade_aus(cm=-75,speed=50) +def kjell_aufgaben(): + iqRobot.fahre_gerade_aus(19,20) + iqRobot.drehe(-45) + iqRobot.fahre_gerade_aus(20,30) + iqRobot.fahre_gerade_aus(-15,30) + iqRobot.drehe(45) + iqRobot.fahre_gerade_aus(37.5,30) + iqRobot.drehe(45) + iqRobot.fahre_gerade_aus(15,50) + iqRobot.fahre_gerade_aus(-20,20) + iqRobot.drehe(-45) + iqRobot.fahre_gerade_aus(20,20) + iqRobot.drehe (90) + iqRobot.fahre_gerade_aus(10,20) + + + iqRobot.drehe(-135) + iqRobot.fahre_gerade_aus(44,30) + iqRobot.drehe(90) + iqRobot.fahre_gerade_aus(10,20) + + #hier fehlt noch was + + iqRobot.fahre_gerade_aus(-10,20) + iqRobot.drehe(-90) + iqRobot.fahre_gerade_aus(42,25) + iqRobot.drehe(90) + iqRobot.fahre_gerade_aus(5,20) + + hologram_aufgabe1() From 6de4a7a70bc624e77e3de818e896681299f0d092 Mon Sep 17 00:00:00 2001 From: Laptop 3 Date: Wed, 22 Nov 2023 18:43:25 +0100 Subject: [PATCH 10/31] Abstandssensor --- iqrobot.py | 16 ++++++++++++++-- main.py | 9 +++++++-- 2 files changed, 21 insertions(+), 4 deletions(-) diff --git a/iqrobot.py b/iqrobot.py index 0ea4b35..768c184 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -1,8 +1,8 @@ -# LEGO type:standard slot:6 autostart +# LEGO type:standard slot:7 autostart 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" @@ -38,6 +38,8 @@ class IQRobot: self.bewegungsSensor: MotionSensor = MotionSensor() + self.abstandsSensor: DistanceSensor = DistanceSensor("D") + def show(self, image: str): ''' @@ -135,6 +137,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 +156,4 @@ print("successfully loaded the IQ Lego teams code :)") + diff --git a/main.py b/main.py index cffdfc7..3a68222 100644 --- a/main.py +++ b/main.py @@ -50,7 +50,7 @@ def importFile(slotid=0, precompiled=False, module_name='importFile'): # 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") +importFile(slotid=7, precompiled=True, module_name="iqrobot") import iqrobot as iq print(iq.HELLO) @@ -72,12 +72,15 @@ dh auch an die Funktionen im importierten Code übergeben werde # Initialisieren des Hubs, der Aktoren und Sensoren hub = PrimeHub() +print("HUB") # Initialisiere Robot Klasse mit unseren Funktionen iqRobot: iq.IQRobot = iq.IQRobot(hub) +print("HUB2") # Führe Funktionen aus unser Robot Klasse aus: iqRobot.show('HAPPY') +print("HAPPY") def hologram_aufgabe1(): iqRobot.fahre_gerade_aus(cm=75,speed=80) @@ -87,4 +90,6 @@ def hologram_aufgabe1(): iqRobot.drehe(-45, False) iqRobot.fahre_gerade_aus(cm=-75,speed=50) -hologram_aufgabe1() +#hologram_aufgabe1() + +iqRobot.fahre_bis_abstand(5) From 83c580e54fd8329f431595e1ab6a442dce48f1e6 Mon Sep 17 00:00:00 2001 From: Laptop 3 Date: Wed, 22 Nov 2023 18:47:07 +0100 Subject: [PATCH 11/31] Port gefixt --- iqrobot.py | 2 +- main.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/iqrobot.py b/iqrobot.py index 768c184..509dcf6 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -1,4 +1,4 @@ -# LEGO type:standard slot:7 autostart +# LEGO type:standard slot:6 autostart import math diff --git a/main.py b/main.py index 3a68222..e63b281 100644 --- a/main.py +++ b/main.py @@ -50,7 +50,7 @@ def importFile(slotid=0, precompiled=False, module_name='importFile'): # Importiere Code aus der Datei "iqrobot.py" # Dateiname und Modulname sollten gleich sein, dann kann man Code Completion nutzen -importFile(slotid=7, precompiled=True, module_name="iqrobot") +importFile(slotid=6, precompiled=True, module_name="iqrobot") import iqrobot as iq print(iq.HELLO) From 4a0671105b197134e3bd08e81571d189e5026768 Mon Sep 17 00:00:00 2001 From: Laptop 3 Date: Wed, 22 Nov 2023 18:48:30 +0100 Subject: [PATCH 12/31] Debug entfernt --- main.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/main.py b/main.py index e63b281..1a4e5bb 100644 --- a/main.py +++ b/main.py @@ -72,15 +72,12 @@ dh auch an die Funktionen im importierten Code übergeben werde # Initialisieren des Hubs, der Aktoren und Sensoren hub = PrimeHub() -print("HUB") # Initialisiere Robot Klasse mit unseren Funktionen iqRobot: iq.IQRobot = iq.IQRobot(hub) -print("HUB2") # Führe Funktionen aus unser Robot Klasse aus: iqRobot.show('HAPPY') -print("HAPPY") def hologram_aufgabe1(): iqRobot.fahre_gerade_aus(cm=75,speed=80) From 01b4c1ebc6c00ca9c92d5baed467655fd5b86a44 Mon Sep 17 00:00:00 2001 From: Laptop 3 Date: Wed, 22 Nov 2023 18:49:27 +0100 Subject: [PATCH 13/31] Docs --- main.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/main.py b/main.py index 1a4e5bb..06a23f1 100644 --- a/main.py +++ b/main.py @@ -87,6 +87,7 @@ def hologram_aufgabe1(): iqRobot.drehe(-45, False) iqRobot.fahre_gerade_aus(cm=-75,speed=50) -#hologram_aufgabe1() +hologram_aufgabe1() +# Fahre, bis 5 Zentimeter vor Gegenstand iqRobot.fahre_bis_abstand(5) From cd47da6f23b60ae4665553185d64e8d883fba635 Mon Sep 17 00:00:00 2001 From: Lars Haferkamp <> Date: Wed, 22 Nov 2023 18:56:19 +0100 Subject: [PATCH 14/31] Gerade aus Fahr Funktion mit PI Regler --- iqrobot.py | 38 ++++++++++++++++++++++++++------------ 1 file changed, 26 insertions(+), 12 deletions(-) diff --git a/iqrobot.py b/iqrobot.py index 0ea4b35..cec7049 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -27,14 +27,16 @@ 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() @@ -47,6 +49,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 +80,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 +119,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 From dc5950178189a8d244b9abca27f6e8beebffabde Mon Sep 17 00:00:00 2001 From: Laptop 3 Date: Wed, 22 Nov 2023 19:00:15 +0100 Subject: [PATCH 15/31] . --- main.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/main.py b/main.py index 06a23f1..cffdfc7 100644 --- a/main.py +++ b/main.py @@ -88,6 +88,3 @@ def hologram_aufgabe1(): iqRobot.fahre_gerade_aus(cm=-75,speed=50) hologram_aufgabe1() - -# Fahre, bis 5 Zentimeter vor Gegenstand -iqRobot.fahre_bis_abstand(5) From 1214813e2ad7e22eed0e5d6751b1953a9b962080 Mon Sep 17 00:00:00 2001 From: Makerlab Laptop 1 Date: Wed, 22 Nov 2023 19:16:58 +0100 Subject: [PATCH 16/31] =?UTF-8?q?H=C3=BCnchen?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- main.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/main.py b/main.py index 296fcf8..3908dbc 100644 --- a/main.py +++ b/main.py @@ -81,7 +81,15 @@ 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(self): + 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) + +huenchenaufgabe() From a20dc320201554476f18ae441fad921f92affbd1 Mon Sep 17 00:00:00 2001 From: "(MakerLab) Laptop 2" Date: Wed, 22 Nov 2023 19:19:01 +0100 Subject: [PATCH 17/31] hologram, druckmaschine und augmented reality --- main.py | 41 ++++++++++++++++++++--------------------- 1 file changed, 20 insertions(+), 21 deletions(-) diff --git a/main.py b/main.py index c1fd649..1ffdb3c 100644 --- a/main.py +++ b/main.py @@ -79,7 +79,7 @@ iqRobot: iq.IQRobot = iq.IQRobot(hub) # Führe Funktionen aus unser Robot Klasse aus: iqRobot.show('HAPPY') -def hologram_aufgabe1(): +def hologram_alt(): iqRobot.fahre_gerade_aus(cm=75,speed=80) iqRobot.drehe(45, False) iqRobot.fahre_gerade_aus(cm=14,speed=70) @@ -87,34 +87,33 @@ def hologram_aufgabe1(): iqRobot.drehe(-45, False) iqRobot.fahre_gerade_aus(cm=-75,speed=50) -def kjell_aufgaben(): - iqRobot.fahre_gerade_aus(19,20) - iqRobot.drehe(-45) + +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,50) - iqRobot.fahre_gerade_aus(-20,20) - iqRobot.drehe(-45) - iqRobot.fahre_gerade_aus(20,20) - iqRobot.drehe (90) - iqRobot.fahre_gerade_aus(10,20) - - + iqRobot.fahre_gerade_aus(15,30) + iqRobot.fahre_gerade_aus(-15,30) + +def augmented_reality(): iqRobot.drehe(-135) - iqRobot.fahre_gerade_aus(44,30) + iqRobot.fahre_gerade_aus(42,30) iqRobot.drehe(90) - iqRobot.fahre_gerade_aus(10,20) - - #hier fehlt noch was - - iqRobot.fahre_gerade_aus(-10,20) + 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(42,25) - iqRobot.drehe(90) iqRobot.fahre_gerade_aus(5,20) -hologram_aufgabe1() +druckmaschine() +hologram() +augmented_reality() \ No newline at end of file From 16601f268a9bacb58e52f7ad60effa7bfebdfd86 Mon Sep 17 00:00:00 2001 From: Lars Haferkamp <> Date: Wed, 22 Nov 2023 20:16:47 +0100 Subject: [PATCH 18/31] Kleiner Fix, kein self da nicht in einer Klasse --- main.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/main.py b/main.py index d5ac33c..da74640 100644 --- a/main.py +++ b/main.py @@ -79,7 +79,7 @@ iqRobot: iq.IQRobot = iq.IQRobot(hub) # Führe Funktionen aus unser Robot Klasse aus: iqRobot.show('HAPPY') -def huenchenaufgabe(self): +def huenchenaufgabe(): iqRobot.fahre_gerade_aus(40,60) iqRobot.drehe(-40,True) iqRobot.fahre_gerade_aus(20,60) From e51ee00cac438e6101132506f6cf587e0aca3c96 Mon Sep 17 00:00:00 2001 From: Harry Kimpel Date: Wed, 29 Nov 2023 16:10:03 +0100 Subject: [PATCH 19/31] =?UTF-8?q?Hinzuf=C3=BCgen=20von=20Kommentaren?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Harry Kimpel --- iqrobot.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/iqrobot.py b/iqrobot.py index d2750f3..09c9836 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -60,6 +60,7 @@ 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 """ if grad == 0 or grad == 360 : print("nichts zu tun") @@ -83,6 +84,12 @@ class IQRobot: 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 + """ self.linker_motor.set_degrees_counted(0) self.bewegungsSensor.reset_yaw_angle() From d76983d4e42bd4638ad6ffd8c250aecff76dab35 Mon Sep 17 00:00:00 2001 From: Harry Kimpel Date: Wed, 29 Nov 2023 16:10:59 +0100 Subject: [PATCH 20/31] =?UTF-8?q?Hinzuf=C3=BCgen=20von=20Kommentaren?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Harry Kimpel --- iqrobot.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/iqrobot.py b/iqrobot.py index 09c9836..41431d4 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -60,7 +60,7 @@ 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 + :param bool with_reset: Parameter, um den Gierwinkel zurückzusetzen, Standard: True """ if grad == 0 or grad == 360 : print("nichts zu tun") From 71209bbf1ca17228ac0e9bbdc5d6bde444df589b Mon Sep 17 00:00:00 2001 From: Harry Kimpel Date: Wed, 29 Nov 2023 17:29:15 +0100 Subject: [PATCH 21/31] =?UTF-8?q?Hinzuf=C3=BCgen=20von=20Kommentaren?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Harry Kimpel --- iqrobot.py | 31 ++++++++++++++++++------------- 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/iqrobot.py b/iqrobot.py index 41431d4..f89d80c 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 @@ -62,11 +60,16 @@ class IQRobot: 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() @@ -75,13 +78,16 @@ 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): """ @@ -96,12 +102,14 @@ class IQRobot: 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 + linker_speed=speed # Gescwindigkeit linker Motor + rechter_speed=speed # Gescwindigkeit rechter Motor + kp = 1.5 # Verstärkungsfaktor + ki = 1.0 # Integralfaktor + sum_cm = 0 # bereits gefahrene Strecke + sum_versatz = 0 # Summe des Versatzes über Zeit + + # wiederhole solange die gefahrene Strecke noch nicht erreicht ist while sum_cm < cm: wait_for_seconds(0.05) sum_cm = self.strecke_gefahren() @@ -116,7 +124,6 @@ class IQRobot: 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 @@ -126,7 +133,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): """ @@ -149,7 +155,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) @@ -158,7 +164,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() From 608480dc449a088e266c61887a1aaf854fa3c7a7 Mon Sep 17 00:00:00 2001 From: Harry Kimpel Date: Wed, 29 Nov 2023 17:32:32 +0100 Subject: [PATCH 22/31] =?UTF-8?q?Hinzuf=C3=BCgen=20von=20Kommentaren?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Harry Kimpel --- iqrobot.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/iqrobot.py b/iqrobot.py index f89d80c..8483357 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -104,8 +104,8 @@ class IQRobot: linker_speed=speed # Gescwindigkeit linker Motor rechter_speed=speed # Gescwindigkeit rechter Motor - kp = 1.5 # Verstärkungsfaktor - ki = 1.0 # Integralfaktor + kp = 1.5 # Verstärkungsfaktor zur Regelung + ki = 1.0 # Integralfaktor zur Regelung sum_cm = 0 # bereits gefahrene Strecke sum_versatz = 0 # Summe des Versatzes über Zeit From f5119caeb7c68d3aef8b8a9ab74a3b1bc65da85e Mon Sep 17 00:00:00 2001 From: Harry Kimpel Date: Wed, 29 Nov 2023 17:54:25 +0100 Subject: [PATCH 23/31] =?UTF-8?q?Anpassung=20fahre=5Fgerade=5Faus=20f?= =?UTF-8?q?=C3=BCr=20negative=20Strecken?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Harry Kimpel --- iqrobot.py | 26 +++++++++++++++++++------- main.py | 3 +-- 2 files changed, 20 insertions(+), 9 deletions(-) diff --git a/iqrobot.py b/iqrobot.py index 8483357..e2e8890 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -96,21 +96,33 @@ class IQRobot: :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 + 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 # Gescwindigkeit linker Motor - rechter_speed=speed # Gescwindigkeit rechter Motor - kp = 1.5 # Verstärkungsfaktor zur Regelung - ki = 1.0 # Integralfaktor zur Regelung - sum_cm = 0 # bereits gefahrene Strecke - sum_versatz = 0 # Summe des Versatzes über Zeit + linker_speed=speed * richtung # Geschwindigkeit linker Motor + rechter_speed=speed * richtung # 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: + while sum_cm < cm * richtung: wait_for_seconds(0.05) sum_cm = self.strecke_gefahren() versatz = self.bewegungsSensor.get_yaw_angle() diff --git a/main.py b/main.py index 7154a34..628ff40 100644 --- a/main.py +++ b/main.py @@ -107,7 +107,7 @@ def hologram(): 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) @@ -119,7 +119,6 @@ def augmented_reality(): iqRobot.fahre_gerade_aus(20,30) iqRobot.drehe(-90) iqRobot.fahre_gerade_aus(5,20) - druckmaschine() hologram() From 2fe0c0a556a437b2d6ed697dc9cdcab98f827996 Mon Sep 17 00:00:00 2001 From: Harry Kimpel Date: Wed, 29 Nov 2023 18:01:27 +0100 Subject: [PATCH 24/31] =?UTF-8?q?Anpassung=20fahre=5Fgerade=5Faus=20f?= =?UTF-8?q?=C3=BCr=20negative=20Strecken?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Harry Kimpel --- iqrobot.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/iqrobot.py b/iqrobot.py index e2e8890..eba783c 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -128,8 +128,8 @@ class IQRobot: 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 * richtung * (1 - abweichung) + rechter_speed = speed * richtung * (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)) From 7956b2bef7501d81e7eef65a352d404dca5650b3 Mon Sep 17 00:00:00 2001 From: Makerlab Laptop 1 Date: Wed, 29 Nov 2023 18:39:23 +0100 Subject: [PATCH 25/31] =?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() From 8304ac28b675b7b6fd4e548b0c3e204b656d9b47 Mon Sep 17 00:00:00 2001 From: Makerlab Laptop 1 Date: Wed, 29 Nov 2023 18:57:07 +0100 Subject: [PATCH 26/31] t --- iqrobot.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/iqrobot.py b/iqrobot.py index eba783c..724a294 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -106,6 +106,7 @@ class IQRobot: richtung = 1 if cm < 0: richtung = -1 + speed = -speed self.linker_motor.set_degrees_counted(0) self.bewegungsSensor.reset_yaw_angle() @@ -113,8 +114,8 @@ class IQRobot: self.antrieb.start_tank(10, 10) self.antrieb.set_default_speed(10) - linker_speed=speed * richtung # Geschwindigkeit linker Motor - rechter_speed=speed * richtung # Geschwindigkeit rechter Motor + 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 @@ -124,12 +125,12 @@ class IQRobot: # 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 * richtung * (1 - abweichung) - rechter_speed = speed * richtung * (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)) From 835b9a69b92b64dbb6548e28b12552acd63beb53 Mon Sep 17 00:00:00 2001 From: Harry Kimpel Date: Wed, 29 Nov 2023 20:51:04 +0100 Subject: [PATCH 27/31] =?UTF-8?q?Anpassung=20fahre=5Fgerade=5Faus=20f?= =?UTF-8?q?=C3=BCr=20negative=20Strecken?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Harry Kimpel --- iqrobot.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/iqrobot.py b/iqrobot.py index 724a294..749d2f8 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -106,7 +106,7 @@ class IQRobot: richtung = 1 if cm < 0: richtung = -1 - speed = -speed + speed = speed * richtung self.linker_motor.set_degrees_counted(0) self.bewegungsSensor.reset_yaw_angle() @@ -114,13 +114,13 @@ class IQRobot: 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 + 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: From 147fb0cc8aa7d0db052e20d3ba3182aadef4c36b Mon Sep 17 00:00:00 2001 From: Harry Kimpel Date: Wed, 29 Nov 2023 21:14:04 +0100 Subject: [PATCH 28/31] =?UTF-8?q?Anpassung=20fahre=5Fgerade=5Faus=20f?= =?UTF-8?q?=C3=BCr=20negative=20Strecken?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Harry Kimpel --- iqrobot.py | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/iqrobot.py b/iqrobot.py index eba783c..749d2f8 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -106,6 +106,7 @@ class IQRobot: richtung = 1 if cm < 0: richtung = -1 + speed = speed * richtung self.linker_motor.set_degrees_counted(0) self.bewegungsSensor.reset_yaw_angle() @@ -113,23 +114,23 @@ class IQRobot: self.antrieb.start_tank(10, 10) self.antrieb.set_default_speed(10) - linker_speed=speed * richtung # Geschwindigkeit linker Motor - rechter_speed=speed * richtung # 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 + 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 * richtung * (1 - abweichung) - rechter_speed = speed * richtung * (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)) From a6ea00afb1d983902dc084296ca86c22a9860887 Mon Sep 17 00:00:00 2001 From: "(MakerLab) Laptop 2" Date: Wed, 3 Jan 2024 16:58:32 +0100 Subject: [PATCH 29/31] =?UTF-8?q?Funktion=20f=C3=BCr=20neue=20Schaufel=20u?= =?UTF-8?q?nd=20Doku?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- iqrobot.py | 130 ++++++++++++++++++++++++++++++++++++++++------------- main.py | 20 +++------ 2 files changed, 105 insertions(+), 45 deletions(-) diff --git a/iqrobot.py b/iqrobot.py index d2750f3..3056463 100644 --- a/iqrobot.py +++ b/iqrobot.py @@ -5,7 +5,8 @@ import math from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor, DistanceSensor from spike.control import wait_for_seconds -HELLO = "HELLO IQ" +print("Lade IQ-Bibliothek") + ''' Wir nutzen "Duck typing", dh wir schreiben hinter jede Variabel mit ':' die Klasse, zB `leftMotor: Motor` @@ -26,6 +27,10 @@ class IQRobot: 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) @@ -52,62 +57,99 @@ class IQRobot: 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): + + 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() + self.bewegungsSensor.reset_yaw_angle() # Gierwinkel zurücksetzen + #steering = 100 if grad > 0 else -100 - toleranz = 0 - aktuell = self.bewegungsSensor.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.antrieb.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.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): - self.linker_motor.set_degrees_counted(0) + """ + 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 - rechter_speed=speed - kp = 1.5 - ki = 1.0 - sum_cm = 0 - sum_versatz = 0 - while sum_cm < cm: - wait_for_seconds(0.05) - sum_cm = self.strecke_gefahren() - versatz = self.bewegungsSensor.get_yaw_angle() + 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 - linker_speed = speed * (1 - abweichung) - rechter_speed = speed * (1 + abweichung) - self.antrieb.start_tank_at_power(int(linker_speed), int(rechter_speed)) + 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() - self.drehe(-versatz) + self.antrieb.stop() # Stoppen + self.drehe(-versatz) # Da Versatz immer != 0, korrigieren def fahre_mit_drehung(self, strecke1, grad, strecke2): @@ -119,11 +161,14 @@ class IQRobot: 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 @@ -142,17 +187,38 @@ 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) - def schaufel(self,prozent): + + 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(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: @@ -161,7 +227,7 @@ class IQRobot: self.antrieb.stop() -print("successfully loaded the IQ Lego teams code :)") +print("Fertig geladen.") diff --git a/main.py b/main.py index 7154a34..bee3bdd 100644 --- a/main.py +++ b/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 #################################### ''' @@ -121,6 +113,8 @@ def augmented_reality(): iqRobot.fahre_gerade_aus(5,20) -druckmaschine() -hologram() -augmented_reality() +#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 ) \ No newline at end of file From 714d50e9965ac4d1ca4b890e9310422be5788b7f Mon Sep 17 00:00:00 2001 From: Makerlab Laptop 1 Date: Wed, 10 Jan 2024 19:15:52 +0100 Subject: [PATCH 30/31] =?UTF-8?q?=C3=84nerungen=20100124?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- iqrobot.py | 8 ++++++-- main.py | 48 +++++++++++++++++++++++++++++++++++++----------- 2 files changed, 43 insertions(+), 13 deletions(-) diff --git a/iqrobot.py b/iqrobot.py index b830cd8..7a37375 100644 --- a/iqrobot.py +++ b/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,7 +46,10 @@ 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): ''' @@ -193,7 +197,7 @@ class IQRobot: """ 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 diff --git a/main.py b/main.py index 723c52b..0f27e6f 100644 --- a/main.py +++ b/main.py @@ -88,32 +88,58 @@ def hologram_alt(): iqRobot.fahre_gerade_aus(cm=-75,speed=50) def druckmaschine(): - iqRobot.fahre_gerade_aus(19,30) + iqRobot.fahre_gerade_aus(19,70) iqRobot.drehe(-45) - iqRobot.fahre_gerade_aus(20,30) - iqRobot.fahre_gerade_aus(-15,30) + 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.fahre_gerade_aus(15,30) + 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(42,30) + iqRobot.fahre_gerade_aus(40,30) iqRobot.drehe(90) iqRobot.fahre_gerade_aus(12,30) - iqRobot.schaufel(-100) - iqRobot.fahre_gerade_aus(-3,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) + #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 ) \ No newline at end of file +#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) + From c82f7f65607fddce9e64e2c0b85fb288508b2978 Mon Sep 17 00:00:00 2001 From: Makerlab Laptop 1 Date: Wed, 21 Feb 2024 17:59:40 +0100 Subject: [PATCH 31/31] 21.02.2024 - 18:00 --- brickiesbot.py | 218 +++++++++++++++++++++++++++++++++++++++++++++++++ iqrobot.py | 84 ++++++++++++++++--- main copy 2.py | 145 ++++++++++++++++++++++++++++++++ main copy.py | 148 +++++++++++++++++++++++++++++++++ main.py | 194 +++++++++++++++++++++++++++++++++---------- main_neu.py | 100 +++++++++++++++++++++++ 6 files changed, 838 insertions(+), 51 deletions(-) create mode 100644 brickiesbot.py create mode 100644 main copy 2.py create mode 100644 main copy.py create mode 100644 main_neu.py 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) + + + +