From 127137e79f9abbc76c6adb4a1cee6cb9752ff92d Mon Sep 17 00:00:00 2001 From: Lars Haferkamp <> Date: Wed, 27 Sep 2023 21:00:25 +0200 Subject: [PATCH 1/3] 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) -- 2.45.2 From 833224793a7903b8fb0c9bca399e11653d079092 Mon Sep 17 00:00:00 2001 From: Lars Haferkamp <> Date: Thu, 5 Oct 2023 08:56:46 +0200 Subject: [PATCH 2/3] 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') -- 2.45.2 From ad4b131af6041d0c2487847c696c74112d89f884 Mon Sep 17 00:00:00 2001 From: unknown Date: Wed, 25 Oct 2023 19:27:02 +0200 Subject: [PATCH 3/3] =?UTF-8?q?Funktionen=20drehe=20angepasst,=20fahre=5Fg?= =?UTF-8?q?erade=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 :)") + + + + + + -- 2.45.2